diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index 8256f28790..a5d4050e70 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/rclcpp/src/rclcpp/executor.cpp @@ -33,6 +33,8 @@ #include "rcutils/logging_macros.h" +#include "tracetools/tracetools.h" + using namespace std::chrono_literals; using rclcpp::exceptions::throw_from_rcl_error; @@ -515,9 +517,15 @@ Executor::execute_any_executable(AnyExecutable & any_exec) return; } if (any_exec.timer) { + TRACEPOINT( + rclcpp_executor_execute, + static_cast(any_exec.timer->get_timer_handle().get())); execute_timer(any_exec.timer); } if (any_exec.subscription) { + TRACEPOINT( + rclcpp_executor_execute, + static_cast(any_exec.subscription->get_subscription_handle().get())); execute_subscription(any_exec.subscription); } if (any_exec.service) { @@ -678,6 +686,7 @@ Executor::execute_client( void Executor::wait_for_work(std::chrono::nanoseconds timeout) { + TRACEPOINT(rclcpp_executor_wait_for_work, timeout.count()); { std::lock_guard guard(mutex_); @@ -827,6 +836,7 @@ Executor::get_next_ready_executable_from_map( const rclcpp::memory_strategy::MemoryStrategy::WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) { + TRACEPOINT(rclcpp_executor_get_next_ready); bool success = false; std::lock_guard guard{mutex_}; // Check the timers to see if there are any that are ready diff --git a/rclcpp/src/rclcpp/subscription_base.cpp b/rclcpp/src/rclcpp/subscription_base.cpp index ba76e5f9bc..472034f362 100644 --- a/rclcpp/src/rclcpp/subscription_base.cpp +++ b/rclcpp/src/rclcpp/subscription_base.cpp @@ -143,6 +143,7 @@ SubscriptionBase::take_type_erased(void * message_out, rclcpp::MessageInfo & mes &message_info_out.get_rmw_message_info(), nullptr // rmw_subscription_allocation_t is unused here ); + TRACEPOINT(rclcpp_take, static_cast(message_out)); if (RCL_RET_SUBSCRIPTION_TAKE_FAILED == ret) { return false; } else if (RCL_RET_OK != ret) {