Add initial instrumentation (#473)

Signed-off-by: Christophe Bedard <fixed-term.christophe.bourquebedard@de.bosch.com>

rcl/CMakeLists.txt

@@ -11,6 +11,7 @@ find_package(rmw REQUIRED)
 find_package(rmw_implementation REQUIRED)
 find_package(rosidl_generator_c REQUIRED)
 find_package(tinydir_vendor REQUIRED)
+find_package(tracetools REQUIRED)
 
 include_directories(include)
 include(cmake/rcl_set_symbol_visibility_hidden.cmake)
@@ -71,6 +72,7 @@ ament_target_dependencies(${PROJECT_NAME}
   "rosidl_generator_c"
   ${RCL_LOGGING_IMPL}
   "tinydir_vendor"
+  "tracetools"
 )
 
 # Causes the visibility macros to use dllexport rather than dllimport,
@@ -102,6 +104,7 @@ ament_export_dependencies(rmw)
 ament_export_dependencies(rcutils)
 ament_export_dependencies(rosidl_generator_c)
 ament_export_dependencies(${RCL_LOGGING_IMPL})
+ament_export_dependencies(tracetools)
 
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)

rcl/package.xml

@@ -20,6 +20,7 @@
   <depend>rmw_implementation</depend>
   <depend>rosidl_generator_c</depend>
   <depend>tinydir_vendor</depend>
+  <depend>tracetools</depend>
 
   <test_depend>ament_cmake_gtest</test_depend>
   <test_depend>ament_cmake_pytest</test_depend>

rcl/src/rcl/client.c

@@ -30,6 +30,7 @@ extern "C"
 #include "rmw/error_handling.h"
 #include "rmw/rmw.h"
 #include "rmw/validate_full_topic_name.h"
+#include "tracetools/tracetools.h"
 
 #include "./common.h"
 
@@ -178,6 +179,12 @@ rcl_client_init(
   atomic_init(&client->impl->sequence_number, 0);
   RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Client initialized");
   ret = RCL_RET_OK;
+  TRACEPOINT(
+    rcl_client_init,
+    (const void *)client,
+    (const void *)node,
+    (const void *)client->impl->rmw_handle,
+    remapped_service_name);
   goto cleanup;
 fail:
   if (client->impl) {

rcl/src/rcl/init.c

@@ -29,6 +29,7 @@ extern "C"
 #include "rcutils/logging_macros.h"
 #include "rcutils/stdatomic_helper.h"
 #include "rmw/error_handling.h"
+#include "tracetools/tracetools.h"
 
 static atomic_uint_least64_t __rcl_next_unique_id = ATOMIC_VAR_INIT(1);
 
@@ -153,6 +154,8 @@ rcl_init(
   // Store the allocator.
   context->impl->allocator = allocator;
 
+  TRACEPOINT(rcl_init, (const void *)context);
+
   return RCL_RET_OK;
 fail:
   __cleanup_context(context);

rcl/src/rcl/node.c

@@ -45,6 +45,7 @@ extern "C"
 #include "rmw/rmw.h"
 #include "rmw/validate_namespace.h"
 #include "rmw/validate_node_name.h"
+#include "tracetools/tracetools.h"
 
 #include "./context_impl.h"
 
@@ -365,6 +366,12 @@ rcl_node_init(
   }
   RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Node initialized");
   ret = RCL_RET_OK;
+  TRACEPOINT(
+    rcl_node_init,
+    (const void *)node,
+    (const void *)rcl_node_get_rmw_handle(node),
+    rcl_node_get_name(node),
+    rcl_node_get_namespace(node));
   goto cleanup;
 fail:
   if (node->impl) {

rcl/src/rcl/publisher.c

@@ -29,6 +29,7 @@ extern "C"
 #include "rcutils/logging_macros.h"
 #include "rmw/error_handling.h"
 #include "rmw/validate_full_topic_name.h"
+#include "tracetools/tracetools.h"
 
 #include "./common.h"
 #include "./publisher_impl.h"
@@ -186,7 +187,13 @@ rcl_publisher_init(
   RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Publisher initialized");
   // context
   publisher->impl->context = node->context;
-
+  TRACEPOINT(
+    rcl_publisher_init,
+    (const void *)publisher,
+    (const void *)node,
+    (const void *)publisher->impl->rmw_handle,
+    remapped_topic_name,
+    options->qos.depth);
   goto cleanup;
 fail:
   if (publisher->impl) {

rcl/src/rcl/service.c

@@ -29,6 +29,7 @@ extern "C"
 #include "rmw/error_handling.h"
 #include "rmw/rmw.h"
 #include "rmw/validate_full_topic_name.h"
+#include "tracetools/tracetools.h"
 
 typedef struct rcl_service_impl_t
 {
@@ -182,6 +183,12 @@ rcl_service_init(
   service->impl->options = *options;
   RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Service initialized");
   ret = RCL_RET_OK;
+  TRACEPOINT(
+    rcl_service_init,
+    (const void *)service,
+    (const void *)node,
+    (const void *)service->impl->rmw_handle,
+    remapped_service_name);
   goto cleanup;
 fail:
   if (service->impl) {

rcl/src/rcl/subscription.c

@@ -27,6 +27,7 @@ extern "C"
 #include "rcutils/logging_macros.h"
 #include "rmw/error_handling.h"
 #include "rmw/validate_full_topic_name.h"
+#include "tracetools/tracetools.h"
 
 #include "./common.h"
 #include "./subscription_impl.h"
@@ -182,6 +183,13 @@ rcl_subscription_init(
   subscription->impl->options = *options;
   RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Subscription initialized");
   ret = RCL_RET_OK;
+  TRACEPOINT(
+    rcl_subscription_init,
+    (const void *)subscription,
+    (const void *)node,
+    (const void *)subscription->impl->rmw_handle,
+    remapped_topic_name,
+    options->qos.depth);
   goto cleanup;
 fail:
   if (subscription->impl) {

rcl/src/rcl/timer.c

@@ -25,6 +25,7 @@ extern "C"
 #include "rcutils/logging_macros.h"
 #include "rcutils/stdatomic_helper.h"
 #include "rcutils/time.h"
+#include "tracetools/tracetools.h"
 
 typedef struct rcl_timer_impl_t
 {
@@ -194,6 +195,7 @@ rcl_timer_init(
     return RCL_RET_BAD_ALLOC;
   }
   *timer->impl = impl;
+  TRACEPOINT(rcl_timer_init, (const void *)timer, period);
   return RCL_RET_OK;
 }