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

…os2#329) * Add pub/sub init, publish and take instrumentation using tracetools Signed-off-by: Christophe Bedard <[email protected]> * Remove redundant publisher field in rmw_publish tracepoint Signed-off-by: Christophe Bedard <[email protected]>
eboasson · May 18, 2022 · c8b9ca6 · c8b9ca6
1 parent a54b75e
commit c8b9ca6
Show file tree

Hide file tree

Showing 3 changed files with 16 additions and 1 deletion.
diff --git a/rmw_cyclonedds_cpp/CMakeLists.txt b/rmw_cyclonedds_cpp/CMakeLists.txt
@@ -29,6 +29,7 @@ find_package(ament_cmake_ros REQUIRED)
 
 find_package(rcutils REQUIRED)
 find_package(rcpputils REQUIRED)
+find_package(tracetools REQUIRED)
 
 #find_package(cyclonedds_cmake_module REQUIRED)
 find_package(CycloneDDS QUIET CONFIG)
@@ -77,6 +78,7 @@ ament_target_dependencies(rmw_cyclonedds_cpp
   "rmw"
   "rmw_dds_common"
   "rosidl_runtime_c"
+  "tracetools"
 )
 
 configure_rmw_library(rmw_cyclonedds_cpp)

diff --git a/rmw_cyclonedds_cpp/package.xml b/rmw_cyclonedds_cpp/package.xml
@@ -18,6 +18,7 @@
   <depend>rosidl_runtime_c</depend>
   <depend>rosidl_typesupport_introspection_c</depend>
   <depend>rosidl_typesupport_introspection_cpp</depend>
+  <depend>tracetools</depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>

diff --git a/rmw_cyclonedds_cpp/src/rmw_node.cpp b/rmw_cyclonedds_cpp/src/rmw_node.cpp
@@ -75,6 +75,8 @@
 
 #include "rosidl_typesupport_cpp/message_type_support.hpp"
 
+#include "tracetools/tracetools.h"
+
 #include "namespace_prefix.hpp"
 
 #include "dds/dds.h"
@@ -1547,6 +1549,7 @@ extern "C" rmw_ret_t rmw_publish(
     return RMW_RET_INVALID_ARGUMENT);
   auto pub = static_cast<CddsPublisher *>(publisher->data);
   assert(pub);
+  TRACEPOINT(rmw_publish, ros_message);
   if (dds_write(pub->enth, ros_message) >= 0) {
     return RMW_RET_OK;
   } else {
@@ -2190,6 +2193,7 @@ extern "C" rmw_publisher_t * rmw_create_publisher(
   }
 
   cleanup_publisher.cancel();
+  TRACEPOINT(rmw_publisher_init, static_cast<const void *>(pub), cddspub->gid.data);
   return pub;
 }
 
@@ -2695,6 +2699,7 @@ extern "C" rmw_subscription_t * rmw_create_subscription(
   }
 
   cleanup_subscription.cancel();
+  TRACEPOINT(rmw_subscription_init, static_cast<const void *>(sub), cddssub->gid.data);
   return sub;
 }
 
@@ -2850,10 +2855,17 @@ static rmw_ret_t rmw_take_int(
         fprintf(stderr, "** sample in history for %.fms\n", static_cast<double>(dt) / 1e6);
       }
 #endif
-      return RMW_RET_OK;
+      goto take_done;
     }
   }
   *taken = false;
+take_done:
+  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;
 }