Add timestamp to rmw_publish tracepoint (#694)

Signed-off-by: Christopher Wecht <[email protected]>
ros2 · Jan 23, 2024 · ba2e66b · ba2e66b
1 parent 952ffb9
commit ba2e66b
Showing 1 changed file with 13 additions and 4 deletions.
diff --git a/rmw_fastrtps_shared_cpp/src/rmw_publish.cpp b/rmw_fastrtps_shared_cpp/src/rmw_publish.cpp
@@ -15,6 +15,8 @@
 #include "fastcdr/Cdr.h"
 #include "fastcdr/FastBuffer.h"
 
+#include "fastdds/rtps/common/Time_t.h"
+
 #include "rmw/allocators.h"
 #include "rmw/error_handling.h"
 #include "rmw/rmw.h"
@@ -57,8 +59,10 @@ __rmw_publish(
   data.type = FASTRTPS_SERIALIZED_DATA_TYPE_ROS_MESSAGE;
   data.data = const_cast<void *>(ros_message);
   data.impl = info->type_support_impl_;
-  TRACETOOLS_TRACEPOINT(rmw_publish, ros_message);
-  if (!info->data_writer_->write(&data)) {
+  eprosima::fastrtps::Time_t stamp;
+  eprosima::fastrtps::Time_t::now(stamp);
+  TRACETOOLS_TRACEPOINT(rmw_publish, publisher, ros_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;
   }
@@ -135,8 +139,13 @@ __rmw_publish_loaned_message(
   RMW_CHECK_ARGUMENT_FOR_NULL(ros_message, RMW_RET_INVALID_ARGUMENT);
 
   auto info = static_cast<CustomPublisherInfo *>(publisher->data);
-  TRACETOOLS_TRACEPOINT(rmw_publish, ros_message);
-  if (!info->data_writer_->write(const_cast<void *>(ros_message))) {
+  eprosima::fastrtps::Time_t stamp;
+  eprosima::fastrtps::Time_t::now(stamp);
+  TRACETOOLS_TRACEPOINT(rmw_publish, publisher, ros_message, stamp.to_ns());
+  if (!info->data_writer_->write_w_timestamp(
+      const_cast<void *>(ros_message),
+      eprosima::fastdds::dds::HANDLE_NIL, stamp))
+  {
     RMW_SET_ERROR_MSG("cannot publish data");
     return RMW_RET_ERROR;
   }