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;
}