diff --git a/rclcpp/CMakeLists.txt b/rclcpp/CMakeLists.txt index 15f9c31706..16a67f0e00 100644 --- a/rclcpp/CMakeLists.txt +++ b/rclcpp/CMakeLists.txt @@ -14,6 +14,7 @@ find_package(rosgraph_msgs REQUIRED) find_package(rosidl_generator_cpp REQUIRED) find_package(rosidl_typesupport_c REQUIRED) find_package(rosidl_typesupport_cpp REQUIRED) +find_package(tracetools REQUIRED) # Default to C++14 if(NOT CMAKE_CXX_STANDARD) @@ -114,7 +115,9 @@ ament_target_dependencies(${PROJECT_NAME} "builtin_interfaces" "rosgraph_msgs" "rosidl_typesupport_cpp" - "rosidl_generator_cpp") + "rosidl_generator_cpp" + "tracetools" +) # Causes the visibility macros to use dllexport rather than dllimport, # which is appropriate when building the dll but not consuming it. @@ -141,6 +144,7 @@ ament_export_dependencies(rosidl_typesupport_cpp) ament_export_dependencies(rosidl_typesupport_c) ament_export_dependencies(rosidl_generator_cpp) ament_export_dependencies(rcl_yaml_param_parser) +ament_export_dependencies(tracetools) if(BUILD_TESTING) find_package(ament_cmake_gtest REQUIRED) diff --git a/rclcpp/include/rclcpp/any_service_callback.hpp b/rclcpp/include/rclcpp/any_service_callback.hpp index 24d1e9f6ce..106e1fd80d 100644 --- a/rclcpp/include/rclcpp/any_service_callback.hpp +++ b/rclcpp/include/rclcpp/any_service_callback.hpp @@ -23,6 +23,7 @@ #include "rclcpp/function_traits.hpp" #include "rclcpp/visibility_control.hpp" #include "rmw/types.h" +#include "tracetools/tracetools.h" namespace rclcpp { @@ -86,6 +87,7 @@ class AnyServiceCallback std::shared_ptr request, std::shared_ptr response) { + TRACEPOINT(callback_start, (const void *)this, false); if (shared_ptr_callback_ != nullptr) { (void)request_header; shared_ptr_callback_(request, response); @@ -94,6 +96,7 @@ class AnyServiceCallback } else { throw std::runtime_error("unexpected request without any callback set"); } + TRACEPOINT(callback_end, (const void *)this); } }; diff --git a/rclcpp/include/rclcpp/any_subscription_callback.hpp b/rclcpp/include/rclcpp/any_subscription_callback.hpp index 86369b126e..26aeaba98e 100644 --- a/rclcpp/include/rclcpp/any_subscription_callback.hpp +++ b/rclcpp/include/rclcpp/any_subscription_callback.hpp @@ -26,6 +26,8 @@ #include "rclcpp/allocator/allocator_common.hpp" #include "rclcpp/function_traits.hpp" #include "rclcpp/visibility_control.hpp" +#include "tracetools/tracetools.h" +#include "tracetools/utils.hpp" namespace rclcpp { @@ -155,6 +157,7 @@ class AnySubscriptionCallback void dispatch( std::shared_ptr message, const rmw_message_info_t & message_info) { + TRACEPOINT(callback_start, (const void *)this, false); if (shared_ptr_callback_) { shared_ptr_callback_(message); } else if (shared_ptr_with_info_callback_) { @@ -174,11 +177,13 @@ class AnySubscriptionCallback } else { throw std::runtime_error("unexpected message without any callback set"); } + TRACEPOINT(callback_end, (const void *)this); } void dispatch_intra_process( ConstMessageSharedPtr message, const rmw_message_info_t & message_info) { + TRACEPOINT(callback_start, (const void *)this, true); if (const_shared_ptr_callback_) { const_shared_ptr_callback_(message); } else if (const_shared_ptr_with_info_callback_) { @@ -195,11 +200,13 @@ class AnySubscriptionCallback throw std::runtime_error("unexpected message without any callback set"); } } + TRACEPOINT(callback_end, (const void *)this); } void dispatch_intra_process( MessageUniquePtr message, const rmw_message_info_t & message_info) { + TRACEPOINT(callback_start, (const void *)this, true); if (shared_ptr_callback_) { typename std::shared_ptr shared_message = std::move(message); shared_ptr_callback_(shared_message); @@ -217,6 +224,7 @@ class AnySubscriptionCallback } else { throw std::runtime_error("unexpected message without any callback set"); } + TRACEPOINT(callback_end, (const void *)this); } bool use_take_shared_method() @@ -224,6 +232,31 @@ class AnySubscriptionCallback return const_shared_ptr_callback_ || const_shared_ptr_with_info_callback_; } + void register_callback_for_tracing() + { + if (shared_ptr_callback_) { + TRACEPOINT( + rclcpp_callback_register, + (const void *)this, + get_symbol(shared_ptr_callback_)); + } else if (shared_ptr_with_info_callback_) { + TRACEPOINT( + rclcpp_callback_register, + (const void *)this, + get_symbol(shared_ptr_with_info_callback_)); + } else if (unique_ptr_callback_) { + TRACEPOINT( + rclcpp_callback_register, + (const void *)this, + get_symbol(unique_ptr_callback_)); + } else if (unique_ptr_with_info_callback_) { + TRACEPOINT( + rclcpp_callback_register, + (const void *)this, + get_symbol(unique_ptr_with_info_callback_)); + } + } + private: std::shared_ptr message_allocator_; MessageDeleter message_deleter_; diff --git a/rclcpp/include/rclcpp/service.hpp b/rclcpp/include/rclcpp/service.hpp index 9dc8d027b0..c098b0b114 100644 --- a/rclcpp/include/rclcpp/service.hpp +++ b/rclcpp/include/rclcpp/service.hpp @@ -33,6 +33,7 @@ #include "rclcpp/logging.hpp" #include "rmw/error_handling.h" #include "rmw/rmw.h" +#include "tracetools/tracetools.h" namespace rclcpp { @@ -154,6 +155,10 @@ class Service : public ServiceBase rclcpp::exceptions::throw_from_rcl_error(ret, "could not create service"); } + TRACEPOINT( + rclcpp_service_callback_added, + (const void *)get_service_handle().get(), + (const void *)&any_callback_); } Service( @@ -172,6 +177,10 @@ class Service : public ServiceBase } service_handle_ = service_handle; + TRACEPOINT( + rclcpp_service_callback_added, + (const void *)get_service_handle().get(), + (const void *)&any_callback_); } Service( @@ -192,6 +201,10 @@ class Service : public ServiceBase // In this case, rcl owns the service handle memory service_handle_ = std::shared_ptr(new rcl_service_t); service_handle_->impl = service_handle->impl; + TRACEPOINT( + rclcpp_service_callback_added, + (const void *)get_service_handle().get(), + (const void *)&any_callback_); } Service() = delete; diff --git a/rclcpp/include/rclcpp/subscription.hpp b/rclcpp/include/rclcpp/subscription.hpp index 5272b32880..3789b05679 100644 --- a/rclcpp/include/rclcpp/subscription.hpp +++ b/rclcpp/include/rclcpp/subscription.hpp @@ -46,6 +46,7 @@ #include "rclcpp/type_support_decl.hpp" #include "rclcpp/visibility_control.hpp" #include "rclcpp/waitable.hpp" +#include "tracetools/tracetools.h" namespace rclcpp { @@ -117,6 +118,14 @@ class Subscription : public SubscriptionBase options.event_callbacks.liveliness_callback, RCL_SUBSCRIPTION_LIVELINESS_CHANGED); } + TRACEPOINT( + rclcpp_subscription_callback_added, + (const void *)get_subscription_handle().get(), + (const void *)&any_callback_); + // The callback object gets copied, so if registration is done too early/before this point + // (e.g. in `AnySubscriptionCallback::set()`), its address won't match any address used later + // in subsequent tracepoints. + any_callback_.register_callback_for_tracing(); } /// Called after construction to continue setup that requires shared_from_this(). diff --git a/rclcpp/include/rclcpp/timer.hpp b/rclcpp/include/rclcpp/timer.hpp index 8a6bc2291f..afeb63b663 100644 --- a/rclcpp/include/rclcpp/timer.hpp +++ b/rclcpp/include/rclcpp/timer.hpp @@ -30,6 +30,8 @@ #include "rclcpp/rate.hpp" #include "rclcpp/utilities.hpp" #include "rclcpp/visibility_control.hpp" +#include "tracetools/tracetools.h" +#include "tracetools/utils.hpp" #include "rcl/error_handling.h" #include "rcl/timer.h" @@ -133,6 +135,14 @@ class GenericTimer : public TimerBase ) : TimerBase(clock, period, context), callback_(std::forward(callback)) { + TRACEPOINT( + rclcpp_timer_callback_added, + (const void *)get_timer_handle().get(), + (const void *)&callback_); + TRACEPOINT( + rclcpp_callback_register, + (const void *)&callback_, + get_symbol(callback_)); } /// Default destructor. @@ -152,7 +162,9 @@ class GenericTimer : public TimerBase if (ret != RCL_RET_OK) { throw std::runtime_error("Failed to notify timer that callback occurred"); } + TRACEPOINT(callback_start, (const void *)&callback_, false); execute_callback_delegate<>(); + TRACEPOINT(callback_end, (const void *)&callback_); } // void specialization diff --git a/rclcpp/package.xml b/rclcpp/package.xml index db46f685ec..48415f734d 100644 --- a/rclcpp/package.xml +++ b/rclcpp/package.xml @@ -28,6 +28,7 @@ rcl_yaml_param_parser rcpputils rmw_implementation + tracetools ament_cmake