Add tracing instrumentation using tracetools

Signed-off-by: Christophe Bedard <[email protected]>

rmw_zenoh_cpp/CMakeLists.txt

@@ -21,6 +21,7 @@ find_package(rcutils REQUIRED)
 find_package(rosidl_typesupport_fastrtps_c REQUIRED)
 find_package(rosidl_typesupport_fastrtps_cpp REQUIRED)
 find_package(rmw REQUIRED)
+find_package(tracetools REQUIRED)
 find_package(zenoh_c_vendor REQUIRED)
 
 add_library(rmw_zenoh_cpp SHARED
@@ -68,6 +69,7 @@ target_link_libraries(rmw_zenoh_cpp
     rosidl_typesupport_fastrtps_c::rosidl_typesupport_fastrtps_c
     rosidl_typesupport_fastrtps_cpp::rosidl_typesupport_fastrtps_cpp
     rmw::rmw
+    tracetools::tracetools
     zenohc::lib
 )
 

rmw_zenoh_cpp/package.xml

@@ -25,6 +25,7 @@
   <depend>rosidl_typesupport_fastrtps_c</depend>
   <depend>rosidl_typesupport_fastrtps_cpp</depend>
   <depend>rmw</depend>
+  <depend>tracetools</depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>

rmw_zenoh_cpp/src/detail/rmw_node_data.cpp

@@ -144,6 +144,7 @@ bool NodeData::create_pub_data(
 
   auto pub_data = PublisherData::make(
     std::move(session),
+    publisher,
     node_,
     entity_->node_info(),
     id_,

rmw_zenoh_cpp/src/detail/rmw_publisher_data.cpp

@@ -35,6 +35,8 @@
 #include "rmw/get_topic_endpoint_info.h"
 #include "rmw/impl/cpp/macros.hpp"
 
+#include "tracetools/tracetools.h"
+
 namespace rmw_zenoh_cpp
 {
 // TODO(yuyuan): SHM, make this configurable
@@ -43,6 +45,7 @@ namespace rmw_zenoh_cpp
 ///=============================================================================
 std::shared_ptr<PublisherData> PublisherData::make(
   const z_loaned_session_t * session,
+  const rmw_publisher_t * const rmw_publisher,
   const rmw_node_t * const node,
   liveliness::NodeInfo node_info,
   std::size_t node_id,
@@ -192,6 +195,7 @@ std::shared_ptr<PublisherData> PublisherData::make(
 
   return std::shared_ptr<PublisherData>(
     new PublisherData{
+      rmw_publisher,
       node,
       std::move(entity),
       std::move(pub),
@@ -204,14 +208,16 @@ std::shared_ptr<PublisherData> PublisherData::make(
 
 ///=============================================================================
 PublisherData::PublisherData(
+  const rmw_publisher_t * const rmw_publisher,
   const rmw_node_t * rmw_node,
   std::shared_ptr<liveliness::Entity> entity,
   z_owned_publisher_t pub,
   std::optional<ze_owned_publication_cache_t> pub_cache,
   z_owned_liveliness_token_t token,
   const void * type_support_impl,
   std::unique_ptr<MessageTypeSupport> type_support)
-: rmw_node_(rmw_node),
+: rmw_publisher_(rmw_publisher),
+  rmw_node_(rmw_node),
   entity_(std::move(entity)),
   pub_(std::move(pub)),
   pub_cache_(std::move(pub_cache)),
@@ -308,9 +314,12 @@ rmw_ret_t PublisherData::publish(
   z_owned_bytes_t attachment;
   uint8_t local_gid[RMW_GID_STORAGE_SIZE];
   entity_->copy_gid(local_gid);
-  create_map_and_set_sequence_num(&attachment, sequence_number_++, local_gid);
+  int64_t source_timestamp = 0;
+  create_map_and_set_sequence_num(&attachment, sequence_number_++, local_gid, &source_timestamp);
   options.attachment = z_move(attachment);
 
+  TRACETOOLS_TRACEPOINT(
+    rmw_publish, static_cast<const void *>(rmw_publisher_), ros_message, source_timestamp);
   z_owned_bytes_t payload;
   if (shmbuf.has_value()) {
     z_bytes_from_shm_mut(&payload, z_move(shmbuf.value()));
@@ -357,11 +366,14 @@ rmw_ret_t PublisherData::publish_serialized_message(
   uint8_t local_gid[RMW_GID_STORAGE_SIZE];
   entity_->copy_gid(local_gid);
   z_owned_bytes_t attachment;
-  create_map_and_set_sequence_num(&attachment, sequence_number_++, local_gid);
+  int64_t source_timestamp = 0;
+  create_map_and_set_sequence_num(&attachment, sequence_number_++, local_gid, &source_timestamp);
   options.attachment = z_move(attachment);
   z_owned_bytes_t payload;
   z_bytes_copy_from_buf(&payload, serialized_message->buffer, data_length);
 
+  TRACETOOLS_TRACEPOINT(
+    rmw_publish, static_cast<const void *>(rmw_publisher_), serialized_message, source_timestamp);
   z_result_t res = z_publisher_put(z_loan(pub_), z_move(payload), &options);
   if (res != Z_OK) {
     if (res == Z_ESESSION_CLOSED) {

rmw_zenoh_cpp/src/detail/rmw_publisher_data.hpp

@@ -43,6 +43,7 @@ class PublisherData final
   // Make a shared_ptr of PublisherData.
   static std::shared_ptr<PublisherData> make(
     const z_loaned_session_t * session,
+    const rmw_publisher_t * const rmw_publisher,
     const rmw_node_t * const node,
     liveliness::NodeInfo node_info,
     std::size_t node_id,
@@ -88,6 +89,7 @@ class PublisherData final
 private:
   // Constructor.
   PublisherData(
+    const rmw_publisher_t * const rmw_publisher,
     const rmw_node_t * rmw_node,
     std::shared_ptr<liveliness::Entity> entity,
     z_owned_publisher_t pub,
@@ -98,6 +100,8 @@ class PublisherData final
 
   // Internal mutex.
   mutable std::mutex mutex_;
+  // The rmw publisher
+  const rmw_publisher_t * rmw_publisher_;
   // The parent node.
   const rmw_node_t * rmw_node_;
   // The Entity generated for the publisher.

rmw_zenoh_cpp/src/detail/zenoh_utils.cpp

@@ -26,13 +26,19 @@ namespace rmw_zenoh_cpp
 {
 ///=============================================================================
 void create_map_and_set_sequence_num(
-  z_owned_bytes_t * out_bytes, int64_t sequence_number, uint8_t gid[RMW_GID_STORAGE_SIZE])
+  z_owned_bytes_t * out_bytes,
+  int64_t sequence_number,
+  uint8_t gid[RMW_GID_STORAGE_SIZE],
+  int64_t * source_timestamp)
 {
   auto now = std::chrono::system_clock::now().time_since_epoch();
   auto now_ns = std::chrono::duration_cast<std::chrono::nanoseconds>(now);
-  int64_t source_timestamp = now_ns.count();
+  int64_t timestamp = now_ns.count();
+  if (nullptr != source_timestamp) {
+    *source_timestamp = timestamp;
+  }
 
-  AttachmentData data(sequence_number, source_timestamp, gid);
+  AttachmentData data(sequence_number, timestamp, gid);
   data.serialize_to_zbytes(out_bytes);
 }
 

rmw_zenoh_cpp/src/detail/zenoh_utils.hpp

@@ -29,7 +29,8 @@ namespace rmw_zenoh_cpp
 void
 create_map_and_set_sequence_num(
   z_owned_bytes_t * out_bytes, int64_t sequence_number,
-  uint8_t gid[RMW_GID_STORAGE_SIZE]);
+  uint8_t gid[RMW_GID_STORAGE_SIZE],
+  int64_t * source_timestamp = nullptr);
 
 ///=============================================================================
 // A class to store the replies to service requests.

rmw_zenoh_cpp/src/rmw_zenoh.cpp

@@ -56,6 +56,8 @@
 #include "rmw/validate_namespace.h"
 #include "rmw/validate_node_name.h"
 
+#include "tracetools/tracetools.h"
+
 namespace
 {
 //==============================================================================
@@ -454,6 +456,14 @@ rmw_create_publisher(
   free_topic_name.cancel();
   free_rmw_publisher.cancel();
 
+  if (TRACETOOLS_TRACEPOINT_ENABLED(rmw_publisher_init)) {
+    rmw_gid_t gid{};
+    // Trigger tracepoint even if we cannot get the GID
+    rmw_ret_t gid_ret = rmw_get_gid_for_publisher(rmw_publisher, &gid);
+    static_cast<void>(gid_ret);
+    TRACETOOLS_DO_TRACEPOINT(
+      rmw_publisher_init, static_cast<const void *>(rmw_publisher), gid.data);
+  }
   return rmw_publisher;
 }
 
@@ -990,6 +1000,11 @@ rmw_create_subscription(
   free_topic_name.cancel();
   free_rmw_subscription.cancel();
 
+  // rmw does not require GIDs for subscriptions, and GIDs in rmw_zenoh are not based on any ID of
+  // the underlying zenoh objects, so there is no need to collect a GID here
+  rmw_gid_t gid{};
+  TRACETOOLS_TRACEPOINT(
+    rmw_subscription_init, static_cast<const void *>(rmw_subscription), gid.data);
   return rmw_subscription;
 }
 
@@ -1134,7 +1149,18 @@ rmw_take(
     static_cast<rmw_zenoh_cpp::SubscriptionData *>(subscription->data);
   RMW_CHECK_ARGUMENT_FOR_NULL(sub_data, RMW_RET_INVALID_ARGUMENT);
 
-  return sub_data->take_one_message(ros_message, nullptr, taken);
+  if (!TRACETOOLS_TRACEPOINT_ENABLED(rmw_take)) {
+    return sub_data->take_one_message(ros_message, nullptr, taken);
+  }
+  rmw_message_info_t message_info{};
+  rmw_ret_t ret = sub_data->take_one_message(ros_message, &message_info, taken);
+  TRACETOOLS_DO_TRACEPOINT(
+    rmw_take,
+    static_cast<const void *>(subscription),
+    static_cast<const void *>(ros_message),
+    message_info.source_timestamp,
+    *taken);
+  return ret;
 }
 
 //==============================================================================
@@ -1163,7 +1189,14 @@ rmw_take_with_info(
     static_cast<rmw_zenoh_cpp::SubscriptionData *>(subscription->data);
   RMW_CHECK_ARGUMENT_FOR_NULL(sub_data, RMW_RET_INVALID_ARGUMENT);
 
-  return sub_data->take_one_message(ros_message, message_info, taken);
+  rmw_ret_t ret = sub_data->take_one_message(ros_message, message_info, taken);
+  TRACETOOLS_TRACEPOINT(
+    rmw_take,
+    static_cast<const void *>(subscription),
+    static_cast<const void *>(ros_message),
+    message_info->source_timestamp,
+    *taken);
+  return ret;
 }
 
 //==============================================================================
@@ -1269,11 +1302,18 @@ __rmw_take_serialized(
     static_cast<rmw_zenoh_cpp::SubscriptionData *>(subscription->data);
   RMW_CHECK_ARGUMENT_FOR_NULL(sub_data, RMW_RET_INVALID_ARGUMENT);
 
-  return sub_data->take_serialized_message(
+  rmw_ret_t ret = sub_data->take_serialized_message(
     serialized_message,
     taken,
     message_info
   );
+  TRACETOOLS_TRACEPOINT(
+    rmw_take,
+    static_cast<const void *>(subscription),
+    static_cast<const void *>(serialized_message),
+    message_info->source_timestamp,
+    *taken);
+  return ret;
 }
 }  // namespace