Instrumentation update

* Add initial instrumentation
* Rename function registration method and elaborate on object copy issue
* Rely on get_symbol overload instead of using enable_if

Signed-off-by: Christophe Bedard <fixed-term.christophe.bourquebedard@de.bosch.com>
Signed-off-by: Christophe Bedard <christophe.bedard@apex.ai>
Signed-off-by: Ingo Luetkebohle <ingo.luetkebohle@de.bosch.com>

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)

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<typename ServiceT::Request> request,
  std::shared_ptr<typename ServiceT::Response> 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);
  }
 };
 

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<MessageT> 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<MessageT> shared_message = std::move(message);
  shared_ptr_callback_(shared_message);
@@ -217,13 +224,39 @@ class AnySubscriptionCallback
  } else {
  throw std::runtime_error("unexpected message without any callback set");
  }
+ TRACEPOINT(callback_end, (const void *)this);
  }
 
  bool use_take_shared_method()
  {
  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<MessageAlloc> message_allocator_;
  MessageDeleter message_deleter_;

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<rcl_service_t>(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;

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().

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<FunctorT>(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

rclcpp/package.xml

@@ -28,6 +28,7 @@
  <depend>rcl_yaml_param_parser</depend>
  <depend>rcpputils</depend>
  <depend>rmw_implementation</depend>
+ <depend>tracetools</depend>
 
  <exec_depend>ament_cmake</exec_depend>