feat(GenericSubscription): Add dispatch subscription callback to gene…

…ric subscription (ros2#7) * added initialization tracepoint to generic_subscription Signed-off-by: yamasaki <[email protected]> * added callback_register Signed-off-by: yamasaki <[email protected]> * added callback_start/end Signed-off-by: yamasaki <[email protected]> * added subscription-side trace point Signed-off-by: ymski <[email protected]> * add rclcpp_publish to generic_publisher Signed-off-by: ymski <[email protected]> * removed unused dispatch_subscription_callback trace point Signed-off-by: ymski <[email protected]> * Update rclcpp/src/rclcpp/generic_subscription.cpp Co-authored-by: isp-uetsuki <[email protected]> * fix messege addr Signed-off-by: ymski <[email protected]> * remove redundant includes Signed-off-by: ymski <[email protected]> --------- Signed-off-by: yamasaki <[email protected]> Signed-off-by: ymski <[email protected]> Co-authored-by: isp-uetsuki <[email protected]>
ymski · Nov 14, 2023 · 197d048 · 197d048
1 parent 58a5228
commit 197d048
Show file tree

Hide file tree

Showing 4 changed files with 33 additions and 9 deletions.
diff --git 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<const void *>(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<const uint64_t>(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
@@ -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<const void *>(&callback_);
+    TRACEPOINT(
+      rclcpp_subscription_init,
+      static_cast<const void *>(get_subscription_handle().get()),
+      static_cast<const void *>(this));
+    TRACEPOINT(
+      rclcpp_subscription_callback_added,
+      static_cast<const void *>(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
@@ -23,6 +23,10 @@ namespace rclcpp
 
 void GenericPublisher::publish(const rclcpp::SerializedMessage & message)
 {
+  TRACEPOINT(
+    rclcpp_publish,
+    static_cast<const void *>(publisher_handle_.get()),
+    static_cast<const void *>(&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
@@ -46,9 +46,20 @@ void GenericSubscription::handle_message(
 void
 GenericSubscription::handle_serialized_message(
   const std::shared_ptr<rclcpp::SerializedMessage> & message,
-  const rclcpp::MessageInfo &)
+  const rclcpp::MessageInfo & message_info)
 {
+    auto callback_ptr = static_cast<const void *>(&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(