diff --git a/rcl/CMakeLists.txt b/rcl/CMakeLists.txt index fffaa237e..e60184371 100644 --- a/rcl/CMakeLists.txt +++ b/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) diff --git a/rcl/package.xml b/rcl/package.xml index e174ab947..f4c133fd6 100644 --- a/rcl/package.xml +++ b/rcl/package.xml @@ -20,6 +20,7 @@ rmw_implementation rosidl_generator_c tinydir_vendor + tracetools ament_cmake_gtest ament_cmake_pytest diff --git a/rcl/src/rcl/client.c b/rcl/src/rcl/client.c index 194610344..889825cd8 100644 --- a/rcl/src/rcl/client.c +++ b/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) { diff --git a/rcl/src/rcl/init.c b/rcl/src/rcl/init.c index 853dc7a3c..ae716a04b 100644 --- a/rcl/src/rcl/init.c +++ b/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); diff --git a/rcl/src/rcl/node.c b/rcl/src/rcl/node.c index 649ebcd06..65ced0db4 100644 --- a/rcl/src/rcl/node.c +++ b/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) { diff --git a/rcl/src/rcl/publisher.c b/rcl/src/rcl/publisher.c index 3b02be76a..930db93a1 100644 --- a/rcl/src/rcl/publisher.c +++ b/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) { diff --git a/rcl/src/rcl/service.c b/rcl/src/rcl/service.c index 70ddd7690..33f4788a4 100644 --- a/rcl/src/rcl/service.c +++ b/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) { diff --git a/rcl/src/rcl/subscription.c b/rcl/src/rcl/subscription.c index 506a5d855..5b352ee6f 100644 --- a/rcl/src/rcl/subscription.c +++ b/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) { diff --git a/rcl/src/rcl/timer.c b/rcl/src/rcl/timer.c index 16e4243b6..df3c21037 100644 --- a/rcl/src/rcl/timer.c +++ b/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; }