Instrumentation update (#789)

* 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 @@ public:
     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 @@ public:
     } 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 @@ public:
   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 @@ public:
     } 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 @@ public:
         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,6 +224,7 @@ public:
     } 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 @@ public:
     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 @@ public:
 
       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 @@ public:
     }
 
     service_handle_ = service_handle;
+    TRACEPOINT(
+      rclcpp_service_callback_added,
+      (const void *)get_service_handle().get(),
+      (const void *)&any_callback_);
   }
 
   Service(
@@ -192,6 +201,10 @@ public:
     // 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 @@ public:
         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 @@ public:
   )
   : 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 @@ public:
     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>