rmw_connext.patch

diff --git a/rmw_connext_cpp/include/rmw_connext_cpp/connext_static_subscriber_info.hpp b/rmw_connext_cpp/include/rmw_connext_cpp/connext_static_subscriber_info.hpp
index 74b912a..a08ec18 100644
--- a/rmw_connext_cpp/include/rmw_connext_cpp/connext_static_subscriber_info.hpp
+++ b/rmw_connext_cpp/include/rmw_connext_cpp/connext_static_subscriber_info.hpp
@@ -37,6 +37,7 @@ struct ConnextStaticSubscriberInfo : ConnextCustomEventInfo
   DDS::DataReader * topic_reader_;
   DDS::ReadCondition * read_condition_;
   const message_type_support_callbacks_t * callbacks_;
+  uint64_t ts_ondataavailable;
   /// Remap the specific RTI Connext DDS DataReader Status to a generic RMW status type.
   /**
    * \param mask input status mask
diff --git a/rmw_connext_cpp/src/rmw_publish.cpp b/rmw_connext_cpp/src/rmw_publish.cpp
index b7632d9..3e7c9b1 100644
--- a/rmw_connext_cpp/src/rmw_publish.cpp
+++ b/rmw_connext_cpp/src/rmw_publish.cpp
@@ -25,9 +25,12 @@
 // include patched generated code from the build folder
 #include "connext_static_serialized_dataSupport.h"
 
+#include <ros2profiling/profiling.h>
+
 bool
 publish(DDS::DataWriter * dds_data_writer, const rcutils_uint8_array_t * cdr_stream)
 {
+  const size_t OFFSET_CDR_TO_MESSAGE = 4;
   ConnextStaticSerializedDataDataWriter * data_writer =
     ConnextStaticSerializedDataDataWriter::narrow(dds_data_writer);
   if (!data_writer) {
@@ -57,6 +60,15 @@ publish(DDS::DataWriter * dds_data_writer, const rcutils_uint8_array_t * cdr_str
     goto cleanup;
   }
 
+  store_profile(data_writer->get_topic()->get_name(),
+		const_cast<void*>((void*)(cdr_stream->buffer+OFFSET_CDR_TO_MESSAGE)),
+		PROFIDX_PUB_DDS_WRITE, "dds_write", NULL);
+
+  /*    for (unsigned int i = 0; i < cdr_stream->buffer_length; i++) {
+    printf("%05d 0x%02x\n", i, cdr_stream->buffer[i]);
+    }
+  */
+
   status = data_writer->write(*instance, DDS::HANDLE_NIL);
 
 cleanup:
@@ -79,6 +91,7 @@ rmw_publish(
   const void * ros_message,
   rmw_publisher_allocation_t * allocation)
 {
+  store_profile(publisher->topic_name, const_cast<void*>(ros_message), PROFIDX_PUB_RMW_PUBLISH, "rmw_publish", NULL);
   (void) allocation;
   RMW_CHECK_FOR_NULL_WITH_MSG(
     publisher, "publisher handle is null",
diff --git a/rmw_connext_cpp/src/rmw_take.cpp b/rmw_connext_cpp/src/rmw_take.cpp
index 47a7b1b..12ab08c 100644
--- a/rmw_connext_cpp/src/rmw_take.cpp
+++ b/rmw_connext_cpp/src/rmw_take.cpp
@@ -27,6 +27,8 @@
 #include "./connext_static_serialized_dataSupport.h"
 #include "./connext_static_serialized_data.h"
 
+#include <ros2profiling/profiling.h>
+
 static bool
 is_local_publication(
   const DDS::SampleInfo & sample_info,
@@ -83,6 +85,7 @@ take(
   DDS::SampleInfoSeq sample_infos;
   bool ignore_sample = false;
 
+  uint64_t ts_enter = get_timestamp();
   DDS::ReturnCode_t status = data_reader->take(
     dds_messages,
     sample_infos,
@@ -90,6 +93,7 @@ take(
     DDS::ANY_SAMPLE_STATE,
     DDS::ANY_VIEW_STATE,
     DDS::ANY_INSTANCE_STATE);
+  uint64_t ts_leave = get_timestamp();
   if (status == DDS::RETCODE_NO_DATA) {
     data_reader->return_loan(dds_messages, sample_infos);
     *taken = false;
@@ -127,6 +131,14 @@ take(
       return false;
     }
     memcpy(cdr_stream->buffer, &dds_messages[0].serialized_data[0], cdr_stream->buffer_length);
+    const size_t OFFSET_CDR_TO_MESSAGE = 4;
+    const char* topic = dds_data_reader->get_topicdescription()->get_name();
+    store_profile(topic,
+		  cdr_stream->buffer+OFFSET_CDR_TO_MESSAGE,
+		  PROFIDX_SUB_DDS_TAKE_ENTER, "dds_take_enter", &ts_enter);
+    store_profile(topic,
+		  cdr_stream->buffer+OFFSET_CDR_TO_MESSAGE,
+		  PROFIDX_SUB_DDS_TAKE_LEAVE, "dds_take_leave", &ts_leave);
 
     *taken = true;
   } else {
@@ -194,6 +206,9 @@ _take(
     return RMW_RET_ERROR;
   }
 
+  const char* topic = topic_reader->get_topicdescription()->get_name();
+  store_profile(topic, ros_message, PROFIDX_SUB_DDS_ONDATA, "dds_ondata", &subscriber_info->ts_ondataavailable);
+
   // the call to take allocates memory for the serialized message
   // we have to free this here again
   free(cdr_stream.buffer);
@@ -353,6 +368,7 @@ rmw_take_with_info(
   rmw_message_info_t * message_info,
   rmw_subscription_allocation_t * allocation)
 {
+  uint64_t ts = get_timestamp();
   RMW_CHECK_FOR_NULL_WITH_MSG(
     message_info, "message info is null",
     return RMW_RET_INVALID_ARGUMENT);
@@ -369,6 +385,9 @@ rmw_take_with_info(
   auto detail = reinterpret_cast<ConnextPublisherGID *>(sender_gid->data);
   detail->publication_handle = sending_publication_handle;
 
+  store_profile(subscription->topic_name, ros_message, PROFIDX_SUB_RMW_TAKE_ENTER, "rmw_take_enter", &ts);
+  store_profile(subscription->topic_name, ros_message, PROFIDX_SUB_RMW_TAKE_LEAVE, "rmw_take_leave", NULL);
+
   return RMW_RET_OK;
 }
 
diff --git a/rmw_connext_shared_cpp/include/rmw_connext_shared_cpp/wait.hpp b/rmw_connext_shared_cpp/include/rmw_connext_shared_cpp/wait.hpp
index 9277dc5..3eaded8 100644
--- a/rmw_connext_shared_cpp/include/rmw_connext_shared_cpp/wait.hpp
+++ b/rmw_connext_shared_cpp/include/rmw_connext_shared_cpp/wait.hpp
@@ -31,6 +31,8 @@
 #include "rmw_connext_shared_cpp/visibility_control.h"
 #include "rmw_connext_shared_cpp/connext_static_event_info.hpp"
 
+#include <ros2profiling/profiling.h>
+
 rmw_ret_t
 __gather_event_conditions(
   rmw_events_t * events,
@@ -331,6 +333,7 @@ wait(
   }
 
   DDS::ReturnCode_t status = dds_wait_set->wait(*active_conditions, timeout);
+  uint64_t ts_ondata = get_timestamp();
 
   if (status != DDS::RETCODE_OK && status != DDS::RETCODE_TIMEOUT) {
     RMW_SET_ERROR_MSG("failed to wait on wait set");
@@ -356,6 +359,7 @@ wait(
       DDS::Long j = 0;
       for (; j < active_conditions->length(); ++j) {
         if ((*active_conditions)[j] == read_condition) {
+	  subscriber_info->ts_ondataavailable = ts_ondata;
           break;
         }
       }