niklas/rcl
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

WIP: backport message flow tracing to foxy

Browse source
This commit is contained in:
Niklas Halle 2025-05-17 12:37:47 +02:00
parent 287ccd9ed0
commit 5da7e19337
5 changed files with 19 additions and 0 deletions
Download patch file Download diff file Expand all files Collapse all files

3
rcl/src/rcl/publisher.c
View file

@ -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);

2
rcl/src/rcl/subscription.c
View file

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

2
rcl_lifecycle/CMakeLists.txt
View file

@ -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"

1
rcl_lifecycle/package.xml
View file

@ -9,6 +9,7 @@
<buildtool_depend>ament_cmake_ros</buildtool_depend>
<build_depend>tracetools</build_depend>
<build_depend>lifecycle_msgs</build_depend>
<build_depend>rcl</build_depend>
<build_depend>rcutils</build_depend>

11
rcl_lifecycle/src/rcl_lifecycle.c
View file

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