Fix topic statistics with intra-process

Author: roman-y-wu

rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp

@@ -31,6 +31,7 @@
 #include "rclcpp/experimental/subscription_intra_process_buffer.hpp"
 #include "rclcpp/qos.hpp"
 #include "rclcpp/type_support_decl.hpp"
+#include "rclcpp/topic_statistics/subscription_topic_statistics.hpp"
 #include "tracetools/tracetools.h"
 
 namespace rclcpp
@@ -70,22 +71,26 @@ class SubscriptionIntraProcess
   using ConstMessageSharedPtr = typename SubscriptionIntraProcessBufferT::ConstDataSharedPtr;
   using MessageUniquePtr = typename SubscriptionIntraProcessBufferT::SubscribedTypeUniquePtr;
   using BufferUniquePtr = typename SubscriptionIntraProcessBufferT::BufferUniquePtr;
+  using SubscriptionTopicStatisticsSharedPtr =
+    std::shared_ptr<rclcpp::topic_statistics::SubscriptionTopicStatistics>;
 
   SubscriptionIntraProcess(
     AnySubscriptionCallback<MessageT, Alloc> callback,
     std::shared_ptr<Alloc> allocator,
     rclcpp::Context::SharedPtr context,
     const std::string & topic_name,
     const rclcpp::QoS & qos_profile,
-    rclcpp::IntraProcessBufferType buffer_type)
+    rclcpp::IntraProcessBufferType buffer_type,
+    SubscriptionTopicStatisticsSharedPtr subscription_topic_statistics = nullptr)
   : SubscriptionIntraProcessBuffer<SubscribedType, SubscribedTypeAlloc,
       SubscribedTypeDeleter, ROSMessageType>(
       std::make_shared<SubscribedTypeAlloc>(*allocator),
       context,
       topic_name,
       qos_profile,
       buffer_type),
-    any_callback_(callback)
+    any_callback_(callback),
+    subscription_topic_statistics_(std::move(subscription_topic_statistics))
   {
     TRACETOOLS_TRACEPOINT(
       rclcpp_subscription_callback_added,
@@ -174,6 +179,13 @@ class SubscriptionIntraProcess
     msg_info.publisher_gid = {0, {0}};
     msg_info.from_intra_process = true;
 
+    std::chrono::time_point<std::chrono::system_clock> now;
+    if (subscription_topic_statistics_) {
+      now = std::chrono::system_clock::now();
+      auto nanos = std::chrono::time_point_cast<std::chrono::nanoseconds>(now);
+      msg_info.source_timestamp = nanos.time_since_epoch().count();
+    }
+
     auto shared_ptr = std::static_pointer_cast<std::pair<ConstMessageSharedPtr, MessageUniquePtr>>(
       data);
 
@@ -185,9 +197,16 @@ class SubscriptionIntraProcess
       any_callback_.dispatch_intra_process(std::move(unique_msg), msg_info);
     }
     shared_ptr.reset();
+
+    if (subscription_topic_statistics_) {
+      const auto nanos = std::chrono::time_point_cast<std::chrono::nanoseconds>(now);
+      const auto time = rclcpp::Time(nanos.time_since_epoch().count());
+      subscription_topic_statistics_->handle_message(msg_info, time);
+    }
   }
 
   AnySubscriptionCallback<MessageT, Alloc> any_callback_;
+  SubscriptionTopicStatisticsSharedPtr subscription_topic_statistics_;
 };
 
 }  // namespace experimental

rclcpp/include/rclcpp/subscription.hpp

@@ -172,7 +172,8 @@ class Subscription : public SubscriptionBase
         context,
         this->get_topic_name(),  // important to get like this, as it has the fully-qualified name
         qos_profile,
-        resolve_intra_process_buffer_type(options_.intra_process_buffer_type, callback));
+        resolve_intra_process_buffer_type(options_.intra_process_buffer_type, callback),
+        subscription_topic_statistics);
       TRACETOOLS_TRACEPOINT(
         rclcpp_subscription_init,
         static_cast<const void *>(get_subscription_handle().get()),

rclcpp/test/rclcpp/test_intra_process_manager.cpp

@@ -25,6 +25,7 @@
 #include "rclcpp/context.hpp"
 #include "rclcpp/macros.hpp"
 #include "rclcpp/qos.hpp"
+#include "rclcpp/topic_statistics/subscription_topic_statistics.hpp"
 #include "rmw/types.h"
 #include "rmw/qos_profiles.h"
 
@@ -376,7 +377,10 @@ class SubscriptionIntraProcess : public SubscriptionIntraProcessBuffer<
 public:
   RCLCPP_SMART_PTR_DEFINITIONS(SubscriptionIntraProcess)
 
-  explicit SubscriptionIntraProcess(const std::string & topic, const rclcpp::QoS & qos)
+  explicit SubscriptionIntraProcess(
+    const std::string & topic,
+    const rclcpp::QoS & qos,
+    std::shared_ptr<rclcpp::topic_statistics::SubscriptionTopicStatistics> = nullptr)
   : SubscriptionIntraProcessBuffer<MessageT, Alloc, Deleter>(topic, qos)
   {
   }