Add tracepoint for publish/subscribe serialized_message (#748)

* Add: tracepoint for generic pub/sub Signed-off-by: h-suzuki <[email protected]> * Fix: correspond to PR 454 Signed-off-by: h-suzuki <[email protected]> * Fix: change write to write_to_timestamp Signed-off-by: h-suzuki <[email protected]> --------- Signed-off-by: h-suzuki <[email protected]>
ros2 · Apr 1, 2024 · c0dab2d · c0dab2d
1 parent ec36951
commit c0dab2d
Show file tree

Hide file tree

Showing 2 changed files with 10 additions and 2 deletions.
diff --git a/rmw_fastrtps_shared_cpp/src/rmw_publish.cpp b/rmw_fastrtps_shared_cpp/src/rmw_publish.cpp
@@ -109,7 +109,10 @@ __rmw_publish_serialized_message(
   data.type = FASTRTPS_SERIALIZED_DATA_TYPE_CDR_BUFFER;
   data.data = &ser;
   data.impl = nullptr;  // not used when type is FASTRTPS_SERIALIZED_DATA_TYPE_CDR_BUFFER
-  if (!info->data_writer_->write(&data)) {
+  eprosima::fastrtps::Time_t stamp;
+  eprosima::fastrtps::Time_t::now(stamp);
+  TRACETOOLS_TRACEPOINT(rmw_publish, publisher, serialized_message, stamp.to_ns());
+  if (!info->data_writer_->write_w_timestamp(&data, eprosima::fastdds::dds::HANDLE_NIL, stamp)) {
     RMW_SET_ERROR_MSG("cannot publish data");
     return RMW_RET_ERROR;
   }

diff --git a/rmw_fastrtps_shared_cpp/src/rmw_take.cpp b/rmw_fastrtps_shared_cpp/src/rmw_take.cpp
@@ -348,7 +348,12 @@ _take_serialized_message(
       break;
     }
   }
-
+  TRACETOOLS_TRACEPOINT(
+    rmw_take,
+    static_cast<const void *>(subscription),
+    static_cast<const void *>(serialized_message),
+    (message_info ? message_info->source_timestamp : 0LL),
+    *taken);
   return RMW_RET_OK;
 }