diff --git a/rclcpp/include/rclcpp/any_subscription_callback.hpp b/rclcpp/include/rclcpp/any_subscription_callback.hpp index 706f4b6c44..422609deab 100644 --- a/rclcpp/include/rclcpp/any_subscription_callback.hpp +++ b/rclcpp/include/rclcpp/any_subscription_callback.hpp @@ -497,14 +497,6 @@ class AnySubscriptionCallback const rclcpp::MessageInfo & message_info) { auto callback_ptr = static_cast(this); - auto rmw_info = message_info.get_rmw_message_info(); - auto source_timestamp = rmw_info.source_timestamp; - TRACEPOINT( - dispatch_subscription_callback, - message.get(), - callback_ptr, - source_timestamp, - static_cast(TimeStampRosMessage::value(*message).second)); TRACEPOINT(callback_start, callback_ptr, false); // Check if the variant is "unset", throw if it is. if (callback_variant_.index() == 0) { diff --git a/rclcpp/include/rclcpp/generic_subscription.hpp b/rclcpp/include/rclcpp/generic_subscription.hpp index 673712eedb..07526745f4 100644 --- a/rclcpp/include/rclcpp/generic_subscription.hpp +++ b/rclcpp/include/rclcpp/generic_subscription.hpp @@ -31,6 +31,7 @@ #include "rclcpp/subscription_base.hpp" #include "rclcpp/typesupport_helpers.hpp" #include "rclcpp/visibility_control.hpp" +#include "tracetools/tracetools.h" namespace rclcpp { @@ -119,6 +120,22 @@ class GenericSubscription : public rclcpp::SubscriptionBase options.event_callbacks.message_lost_callback, RCL_SUBSCRIPTION_MESSAGE_LOST); } + auto callback_ptr = static_cast(&callback_); + TRACEPOINT( + rclcpp_subscription_init, + static_cast(get_subscription_handle().get()), + static_cast(this)); + TRACEPOINT( + rclcpp_subscription_callback_added, + static_cast(this), + callback_ptr); + +#ifndef TRACETOOLS_DISABLED + TRACEPOINT( + rclcpp_callback_register, + callback_ptr, + tracetools::get_symbol(callback)); +#endif } RCLCPP_PUBLIC diff --git a/rclcpp/src/rclcpp/generic_publisher.cpp b/rclcpp/src/rclcpp/generic_publisher.cpp index 87ace1eb19..cd92e62d6d 100644 --- a/rclcpp/src/rclcpp/generic_publisher.cpp +++ b/rclcpp/src/rclcpp/generic_publisher.cpp @@ -23,6 +23,10 @@ namespace rclcpp void GenericPublisher::publish(const rclcpp::SerializedMessage & message) { + TRACEPOINT( + rclcpp_publish, + static_cast(publisher_handle_.get()), + static_cast(&message.get_rcl_serialized_message())); auto return_code = rcl_publish_serialized_message( get_publisher_handle().get(), &message.get_rcl_serialized_message(), NULL); diff --git a/rclcpp/src/rclcpp/generic_subscription.cpp b/rclcpp/src/rclcpp/generic_subscription.cpp index cc50955773..987610cab7 100644 --- a/rclcpp/src/rclcpp/generic_subscription.cpp +++ b/rclcpp/src/rclcpp/generic_subscription.cpp @@ -46,9 +46,20 @@ void GenericSubscription::handle_message( void GenericSubscription::handle_serialized_message( const std::shared_ptr & message, - const rclcpp::MessageInfo &) + const rclcpp::MessageInfo & message_info) { + auto callback_ptr = static_cast(&callback_); + auto rmw_info = message_info.get_rmw_message_info(); + auto source_timestamp = rmw_info.source_timestamp; + TRACEPOINT( + dispatch_subscription_callback, + message.get(), + callback_ptr, + source_timestamp, + 0); + TRACEPOINT(callback_start, callback_ptr, false); callback_(message); + TRACEPOINT(callback_end, callback_ptr); } void GenericSubscription::handle_loaned_message(