Add pub/sub init, publish and take instrumentation using tracetools

Signed-off-by: Christophe Bedard <bedard.christophe@gmail.com>

rmw_fastrtps_cpp/CMakeLists.txt

@@ -35,6 +35,7 @@ find_package(rcpputils REQUIRED)
 find_package(rcutils REQUIRED)
 find_package(rmw_dds_common REQUIRED)
 find_package(rmw_fastrtps_shared_cpp REQUIRED)
+find_package(tracetools REQUIRED)
 
 find_package(fastrtps_cmake_module REQUIRED)
 find_package(fastcdr REQUIRED CONFIG)
@@ -105,6 +106,7 @@ ament_target_dependencies(rmw_fastrtps_cpp
   "rmw_fastrtps_shared_cpp"
   "rmw"
   "rosidl_runtime_c"
+  "tracetools"
 )
 
 target_link_libraries(rmw_fastrtps_cpp
@@ -130,6 +132,7 @@ ament_export_dependencies(rmw_fastrtps_shared_cpp)
 ament_export_dependencies(rosidl_runtime_c)
 ament_export_dependencies(rosidl_typesupport_fastrtps_c)
 ament_export_dependencies(rosidl_typesupport_fastrtps_cpp)
+ament_export_dependencies(tracetools)
 
 register_rmw_implementation(
   "c:rosidl_typesupport_c:rosidl_typesupport_fastrtps_c:rosidl_typesupport_introspection_c"

rmw_fastrtps_cpp/package.xml

@@ -29,6 +29,7 @@
   <build_depend>rosidl_runtime_cpp</build_depend>
   <build_depend>rosidl_typesupport_fastrtps_c</build_depend>
   <build_depend>rosidl_typesupport_fastrtps_cpp</build_depend>
+  <build_depend>tracetools</build_depend>
 
   <build_export_depend>fastcdr</build_export_depend>
   <build_export_depend>fastrtps</build_export_depend>
@@ -41,11 +42,13 @@
   <build_export_depend>rosidl_runtime_cpp</build_export_depend>
   <build_export_depend>rosidl_typesupport_fastrtps_c</build_export_depend>
   <build_export_depend>rosidl_typesupport_fastrtps_cpp</build_export_depend>
+  <build_export_depend>tracetools</build_export_depend>
 
   <exec_depend>rcpputils</exec_depend>
   <exec_depend>rcutils</exec_depend>
   <exec_depend>rmw</exec_depend>
   <exec_depend>rmw_fastrtps_shared_cpp</exec_depend>
+  <exec_depend>tracetools</exec_depend>
 
   <test_depend>ament_cmake_gtest</test_depend>
   <test_depend>ament_lint_auto</test_depend>

rmw_fastrtps_cpp/src/publisher.cpp

@@ -47,6 +47,8 @@
 #include "rmw_fastrtps_cpp/identifier.hpp"
 #include "rmw_fastrtps_cpp/publisher.hpp"
 
+#include "tracetools/tracetools.h"
+
 #include "type_support_common.hpp"
 
 using DataSharingKind = eprosima::fastdds::dds::DataSharingKind;
@@ -321,5 +323,9 @@ rmw_fastrtps_cpp::create_publisher(
   cleanup_datawriter.cancel();
   cleanup_info.cancel();
 
+  TRACEPOINT(
+    rmw_publisher_init,
+    static_cast<const void *>(rmw_publisher),
+    info->publisher_gid.data);
   return rmw_publisher;
 }

rmw_fastrtps_cpp/src/subscription.cpp

@@ -49,6 +49,8 @@
 #include "rmw_fastrtps_cpp/identifier.hpp"
 #include "rmw_fastrtps_cpp/subscription.hpp"
 
+#include "tracetools/tracetools.h"
+
 #include "type_support_common.hpp"
 
 using PropertyPolicyHelper = eprosima::fastrtps::rtps::PropertyPolicyHelper;
@@ -336,6 +338,11 @@ create_subscription(
   cleanup_rmw_subscription.cancel();
   cleanup_datareader.cancel();
   cleanup_info.cancel();
+
+  TRACEPOINT(
+    rmw_subscription_init,
+    static_cast<const void *>(rmw_subscription),
+    info->subscription_gid_.data);
   return rmw_subscription;
 }
 }  // namespace rmw_fastrtps_cpp

rmw_fastrtps_shared_cpp/CMakeLists.txt

@@ -40,6 +40,7 @@ find_package(rcutils REQUIRED)
 find_package(rmw_dds_common REQUIRED)
 find_package(rosidl_typesupport_introspection_c REQUIRED)
 find_package(rosidl_typesupport_introspection_cpp REQUIRED)
+find_package(tracetools REQUIRED)
 
 find_package(fastrtps_cmake_module REQUIRED)
 find_package(fastcdr REQUIRED CONFIG)
@@ -109,6 +110,7 @@ ament_target_dependencies(rmw_fastrtps_shared_cpp
   "rmw_dds_common"
   "rosidl_typesupport_introspection_c"
   "rosidl_typesupport_introspection_cpp"
+  "tracetools"
 )
 
 # Causes the visibility macros to use dllexport rather than dllimport,
@@ -130,6 +132,7 @@ ament_export_dependencies(rmw)
 ament_export_dependencies(rmw_dds_common)
 ament_export_dependencies(rosidl_typesupport_introspection_c)
 ament_export_dependencies(rosidl_typesupport_introspection_cpp)
+ament_export_dependencies(tracetools)
 
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)

rmw_fastrtps_shared_cpp/package.xml

@@ -24,6 +24,7 @@
   <build_depend>rmw_dds_common</build_depend>
   <build_depend>rosidl_typesupport_introspection_c</build_depend>
   <build_depend>rosidl_typesupport_introspection_cpp</build_depend>
+  <build_depend>tracetools</build_depend>
 
   <build_export_depend>fastcdr</build_export_depend>
   <build_export_depend>fastrtps</build_export_depend>
@@ -34,6 +35,7 @@
   <build_export_depend>rmw_dds_common</build_export_depend>
   <build_export_depend>rosidl_typesupport_introspection_c</build_export_depend>
   <build_export_depend>rosidl_typesupport_introspection_cpp</build_export_depend>
+  <build_export_depend>tracetools</build_export_depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>

rmw_fastrtps_shared_cpp/src/rmw_publish.cpp

@@ -24,6 +24,8 @@
 #include "rmw_fastrtps_shared_cpp/custom_publisher_info.hpp"
 #include "rmw_fastrtps_shared_cpp/TypeSupport.hpp"
 
+#include "tracetools/tracetools.h"
+
 namespace rmw_fastrtps_shared_cpp
 {
 rmw_ret_t
@@ -55,6 +57,7 @@ __rmw_publish(
   data.is_cdr_buffer = false;
   data.data = const_cast<void *>(ros_message);
   data.impl = info->type_support_impl_;
+  TRACEPOINT(rmw_publish, ros_message);
   if (!info->data_writer_->write(&data)) {
     RMW_SET_ERROR_MSG("cannot publish data");
     return RMW_RET_ERROR;

rmw_fastrtps_shared_cpp/src/rmw_take.cpp

@@ -34,6 +34,8 @@
 #include "rmw_fastrtps_shared_cpp/TypeSupport.hpp"
 #include "rmw_fastrtps_shared_cpp/utils.hpp"
 
+#include "tracetools/tracetools.h"
+
 namespace rmw_fastrtps_shared_cpp
 {
 
@@ -114,6 +116,12 @@ _take(
     }
   }
 
+  TRACEPOINT(
+    rmw_take,
+    static_cast<const void *>(subscription),
+    static_cast<const void *>(ros_message),
+    (message_info ? message_info->source_timestamp : 0LL),
+    *taken);
   return RMW_RET_OK;
 }