applied tracepoints for ring_buffer (#2091)

applied tracepoints for intra_publish
add tracepoints for linking buffer and subscription

Signed-off-by: Kodai Yamasaki <[email protected]>

Author: ymski

rclcpp/include/rclcpp/experimental/buffers/intra_process_buffer.hpp

@@ -24,6 +24,7 @@
 #include "rclcpp/allocator/allocator_deleter.hpp"
 #include "rclcpp/experimental/buffers/buffer_implementation_base.hpp"
 #include "rclcpp/macros.hpp"
+#include "tracetools/tracetools.h"
 
 namespace rclcpp
 {
@@ -94,6 +95,10 @@ class TypedIntraProcessBuffer : public IntraProcessBuffer<MessageT, Alloc, Messa
 
     buffer_ = std::move(buffer_impl);
 
+    TRACEPOINT(
+      rclcpp_buffer_to_ipb,
+      static_cast<const void *>(buffer_.get()),
+      static_cast<const void *>(this));
     if (!allocator) {
       message_allocator_ = std::make_shared<MessageAlloc>();
     } else {

rclcpp/include/rclcpp/experimental/buffers/ring_buffer_implementation.hpp

@@ -25,6 +25,7 @@
 #include "rclcpp/logging.hpp"
 #include "rclcpp/macros.hpp"
 #include "rclcpp/visibility_control.hpp"
+#include "tracetools/tracetools.h"
 
 namespace rclcpp
 {
@@ -51,6 +52,7 @@ class RingBufferImplementation : public BufferImplementationBase<BufferT>
     if (capacity == 0) {
       throw std::invalid_argument("capacity must be a positive, non-zero value");
     }
+    TRACEPOINT(rclcpp_construct_ring_buffer, static_cast<const void *>(this), capacity_);
   }
 
   virtual ~RingBufferImplementation() {}
@@ -67,6 +69,12 @@ class RingBufferImplementation : public BufferImplementationBase<BufferT>
 
     write_index_ = next_(write_index_);
     ring_buffer_[write_index_] = std::move(request);
+    TRACEPOINT(
+      rclcpp_ring_buffer_enqueue,
+      static_cast<const void *>(this),
+      write_index_,
+      size_ + 1,
+      is_full_());
 
     if (is_full_()) {
       read_index_ = next_(read_index_);
@@ -90,6 +98,11 @@ class RingBufferImplementation : public BufferImplementationBase<BufferT>
     }
 
     auto request = std::move(ring_buffer_[read_index_]);
+    TRACEPOINT(
+      rclcpp_ring_buffer_dequeue,
+      static_cast<const void *>(this),
+      read_index_,
+      size_ - 1);
     read_index_ = next_(read_index_);
 
     size_--;
@@ -135,7 +148,10 @@ class RingBufferImplementation : public BufferImplementationBase<BufferT>
     return is_full_();
   }
 
-  void clear() {}
+  void clear()
+  {
+    TRACEPOINT(rclcpp_ring_buffer_clear, static_cast<const void *>(this));
+  }
 
 private:
   /// Get the next index value for the ring buffer

rclcpp/include/rclcpp/experimental/subscription_intra_process_buffer.hpp

@@ -31,6 +31,8 @@
 #include "rclcpp/qos.hpp"
 #include "rclcpp/type_support_decl.hpp"
 
+#include "tracetools/tracetools.h"
+
 namespace rclcpp
 {
 namespace experimental
@@ -91,6 +93,10 @@ class SubscriptionIntraProcessBuffer : public SubscriptionROSMsgIntraProcessBuff
       buffer_type,
       qos_profile,
       std::make_shared<Alloc>(subscribed_type_allocator_));
+    TRACEPOINT(
+      rclcpp_ipb_to_subscription,
+      static_cast<const void *>(buffer_.get()),
+      static_cast<const void *>(this));
   }
 
   bool

rclcpp/include/rclcpp/publisher.hpp

@@ -484,6 +484,10 @@ class Publisher : public PublisherBase
     if (!msg) {
       throw std::runtime_error("cannot publish msg which is a null pointer");
     }
+    TRACEPOINT(
+      rclcpp_intra_publish,
+      static_cast<const void *>(publisher_handle_.get()),
+      msg.get());
 
     ipm->template do_intra_process_publish<PublishedType, ROSMessageType, AllocatorT>(
       intra_process_publisher_id_,
@@ -502,6 +506,10 @@ class Publisher : public PublisherBase
     if (!msg) {
       throw std::runtime_error("cannot publish msg which is a null pointer");
     }
+    TRACEPOINT(
+      rclcpp_intra_publish,
+      static_cast<const void *>(publisher_handle_.get()),
+      msg.get());
 
     ipm->template do_intra_process_publish<ROSMessageType, ROSMessageType, AllocatorT>(
       intra_process_publisher_id_,
@@ -521,6 +529,10 @@ class Publisher : public PublisherBase
     if (!msg) {
       throw std::runtime_error("cannot publish msg which is a null pointer");
     }
+    TRACEPOINT(
+      rclcpp_intra_publish,
+      static_cast<const void *>(publisher_handle_.get()),
+      msg.get());
 
     return ipm->template do_intra_process_publish_and_return_shared<ROSMessageType, ROSMessageType,
              AllocatorT>(