Add tracepoint for generic publisher/subscriber

Signed-off-by: h-suzuki <[email protected]>
ymski · Mar 5, 2024 · 19752e3 · 19752e3
1 parent b007204
commit 19752e3
Show file tree

Hide file tree

Showing 3 changed files with 23 additions and 11 deletions.
diff --git a/rclcpp/include/rclcpp/generic_subscription.hpp b/rclcpp/include/rclcpp/generic_subscription.hpp
@@ -84,13 +84,23 @@ class GenericSubscription : public rclcpp::SubscriptionBase
       options.event_callbacks,
       options.use_default_callbacks,
       DeliveredMessageKind::SERIALIZED_MESSAGE),
-    callback_([callback](
-        std::shared_ptr<const rclcpp::SerializedMessage> serialized_message,
-        const rclcpp::MessageInfo & message_info) mutable {
-        callback.dispatch(serialized_message, message_info);
-      }),
+    any_callback_(callback),
     ts_lib_(ts_lib)
-  {}
+  {
+    auto callback_ptr = static_cast<const void *>(&any_callback_);
+    TRACETOOLS_TRACEPOINT(
+      rclcpp_subscription_init,
+      static_cast<const void *>(get_subscription_handle().get()),
+      static_cast<const void *>(this));
+    TRACETOOLS_TRACEPOINT(
+      rclcpp_subscription_callback_added,
+      static_cast<const void *>(this),
+      callback_ptr);
+
+#ifndef TRACETOOLS_DISABLED
+    any_callback_.register_callback_for_tracing();
+#endif
+  }
 
   RCLCPP_PUBLIC
   virtual ~GenericSubscription() = default;
@@ -153,10 +163,8 @@ class GenericSubscription : public rclcpp::SubscriptionBase
 
 private:
   RCLCPP_DISABLE_COPY(GenericSubscription)
-
-  std::function<void(
-      std::shared_ptr<const rclcpp::SerializedMessage>,
-      const rclcpp::MessageInfo)> callback_;
+
+  AnySubscriptionCallback<rclcpp::SerializedMessage, std::allocator<void>> any_callback_;
   // The type support library should stay loaded, so it is stored in the GenericSubscription
   std::shared_ptr<rcpputils::SharedLibrary> ts_lib_;
 };

diff --git a/rclcpp/src/rclcpp/generic_publisher.cpp b/rclcpp/src/rclcpp/generic_publisher.cpp
@@ -23,6 +23,9 @@ namespace rclcpp
 
 void GenericPublisher::publish(const rclcpp::SerializedMessage & message)
 {
+  TRACETOOLS_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);
 
@@ -68,6 +71,7 @@ void GenericPublisher::deserialize_message(
 
 void GenericPublisher::publish_loaned_message(void * loaned_message)
 {
+  TRACETOOLS_TRACEPOINT(rclcpp_publish, nullptr, static_cast<const void *>(loaned_message));
   auto return_code = rcl_publish_loaned_message(
     get_publisher_handle().get(), loaned_message, NULL);
 

diff --git a/rclcpp/src/rclcpp/generic_subscription.cpp b/rclcpp/src/rclcpp/generic_subscription.cpp
@@ -51,7 +51,7 @@ GenericSubscription::handle_serialized_message(
   const std::shared_ptr<rclcpp::SerializedMessage> & message,
   const rclcpp::MessageInfo & message_info)
 {
-  callback_(message, message_info);
+  any_callback_.dispatch(message,message_info);
 }
 
 void