From 19752e3757d6cf13497f1b9c57cfe9132def7e3e Mon Sep 17 00:00:00 2001
From: h-suzuki <h-suzuki@isp.co.jp>
Date: Wed, 10 Jan 2024 14:40:58 +0900
Subject: [PATCH] Add tracepoint for generic publisher/subscriber

Signed-off-by: h-suzuki <h-suzuki@isp.co.jp>
---
 .../include/rclcpp/generic_subscription.hpp   | 28 ++++++++++++-------
 rclcpp/src/rclcpp/generic_publisher.cpp       |  4 +++
 rclcpp/src/rclcpp/generic_subscription.cpp    |  2 +-
 3 files changed, 23 insertions(+), 11 deletions(-)

diff --git a/rclcpp/include/rclcpp/generic_subscription.hpp b/rclcpp/include/rclcpp/generic_subscription.hpp
index e9bf79deea..76fb73bea8 100644
--- 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
index 87ace1eb19..7ab0785b8a 100644
--- 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
index 2d2be61277..4878489826 100644
--- 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