diff --git a/rcl/src/rcl/publisher.c b/rcl/src/rcl/publisher.c index 59da94d..2056e65 100644 --- a/rcl/src/rcl/publisher.c +++ b/rcl/src/rcl/publisher.c @@ -315,6 +315,7 @@ rcl_publish( return RCL_RET_PUBLISHER_INVALID; // error already set } RCL_CHECK_ARGUMENT_FOR_NULL(ros_message, RCL_RET_INVALID_ARGUMENT); + TRACEPOINT(rcl_publish, (const void *)publisher, (const void *)ros_message); if (rmw_publish(publisher->impl->rmw_handle, ros_message, allocation) != RMW_RET_OK) { RCL_SET_ERROR_MSG(rmw_get_error_string().str); return RCL_RET_ERROR; @@ -332,6 +333,7 @@ rcl_publish_serialized_message( return RCL_RET_PUBLISHER_INVALID; // error already set } RCL_CHECK_ARGUMENT_FOR_NULL(serialized_message, RCL_RET_INVALID_ARGUMENT); + TRACEPOINT(rcl_publish, (const void *)publisher, (const void *)serialized_message); rmw_ret_t ret = rmw_publish_serialized_message( publisher->impl->rmw_handle, serialized_message, allocation); if (ret != RMW_RET_OK) { @@ -354,6 +356,7 @@ rcl_publish_loaned_message( return RCL_RET_PUBLISHER_INVALID; // error already set } RCL_CHECK_ARGUMENT_FOR_NULL(ros_message, RCL_RET_INVALID_ARGUMENT); + TRACEPOINT(rcl_publish, (const void *)publisher, (const void *)ros_message); rmw_ret_t ret = rmw_publish_loaned_message(publisher->impl->rmw_handle, ros_message, allocation); if (ret != RMW_RET_OK) { RCL_SET_ERROR_MSG(rmw_get_error_string().str); diff --git a/rcl/src/rcl/subscription.c b/rcl/src/rcl/subscription.c index f3628b8..4a2d71c 100644 --- a/rcl/src/rcl/subscription.c +++ b/rcl/src/rcl/subscription.c @@ -290,6 +290,7 @@ rcl_take( } RCUTILS_LOG_DEBUG_NAMED( ROS_PACKAGE_NAME, "Subscription take succeeded: %s", taken ? "true" : "false"); + TRACEPOINT(rcl_take, (const void *)ros_message); if (!taken) { return RCL_RET_SUBSCRIPTION_TAKE_FAILED; } @@ -369,6 +370,7 @@ rcl_take_serialized_message( } RCUTILS_LOG_DEBUG_NAMED( ROS_PACKAGE_NAME, "Subscription serialized take succeeded: %s", taken ? "true" : "false"); + TRACEPOINT(rcl_take, (const void *)serialized_message); if (!taken) { return RCL_RET_SUBSCRIPTION_TAKE_FAILED; } diff --git a/rcl_lifecycle/CMakeLists.txt b/rcl_lifecycle/CMakeLists.txt index 9344ba7..561aba5 100644 --- a/rcl_lifecycle/CMakeLists.txt +++ b/rcl_lifecycle/CMakeLists.txt @@ -4,6 +4,7 @@ project(rcl_lifecycle) find_package(ament_cmake_ros REQUIRED) +find_package(tracetools REQUIRED) find_package(lifecycle_msgs REQUIRED) find_package(rcl REQUIRED) find_package(rcutils REQUIRED) @@ -45,6 +46,7 @@ target_include_directories(${PROJECT_NAME} PUBLIC # specific order: dependents before dependencies ament_target_dependencies(rcl_lifecycle + "tracetools" "lifecycle_msgs" "rcl" "rcutils" diff --git a/rcl_lifecycle/package.xml b/rcl_lifecycle/package.xml index a9f43da..6309279 100644 --- a/rcl_lifecycle/package.xml +++ b/rcl_lifecycle/package.xml @@ -9,6 +9,7 @@ ament_cmake_ros + tracetools lifecycle_msgs rcl rcutils diff --git a/rcl_lifecycle/src/rcl_lifecycle.c b/rcl_lifecycle/src/rcl_lifecycle.c index 56dc6b4..14f8bba 100644 --- a/rcl_lifecycle/src/rcl_lifecycle.c +++ b/rcl_lifecycle/src/rcl_lifecycle.c @@ -33,6 +33,8 @@ extern "C" #include "rcl_lifecycle/default_state_machine.h" #include "rcl_lifecycle/transition_map.h" +#include "tracetools/tracetools.h" + #include "./com_interface.h" rcl_lifecycle_state_t @@ -242,6 +244,10 @@ rcl_lifecycle_state_machine_init( } } + TRACEPOINT( + rcl_lifecycle_state_machine_init, + (const void *)node_handle, + (const void *)state_machine); return RCL_RET_OK; } @@ -369,6 +375,11 @@ _trigger_transition( } } + TRACEPOINT( + rcl_lifecycle_transition, + (const void *)state_machine, + transition->start->label, + state_machine->current_state->label); return RCL_RET_OK; }