Fix topic statistics for IPC subscriptions

If use_intra_process_comms is enabled, messages are delivered via
SubscriptionIntraProcess::execute_impl() which previously never called
the topic statistics handler, this causes all statistics to report NaN values.

The fix here puts a type-erased StatsHandlerFn in SubscriptionIntraProcess
and wires it up from the Subscription via a  lambda. Using std::function avoids
a circular include chain that would occur if subscription_topic_statistics.hpp
were included directly from subscription_intra_process.hpp via
publisher.hpp/callback_group.hpp.

The source_timestamp is set to the receive time so that message_age reports
0ms rather than an un-initialised value. IPC delivery has little/no expected transport
latency, by definition so near-zero age is expected.

Fixes #2911

Signed-off-by: jayyoung <jay.young@gmail.com>

Author: jayyoung

rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp

@@ -17,6 +17,7 @@
 
 #include <rmw/types.h>
 
+#include <functional>
 #include <memory>
 #include <stdexcept>
 #include <string>
@@ -29,7 +30,9 @@
 #include "rclcpp/context.hpp"
 #include "rclcpp/experimental/buffers/intra_process_buffer.hpp"
 #include "rclcpp/experimental/subscription_intra_process_buffer.hpp"
+#include "rclcpp/logging.hpp"
 #include "rclcpp/qos.hpp"
+#include "rclcpp/time.hpp"
 #include "rclcpp/type_support_decl.hpp"
 #include "tracetools/tracetools.h"
 
@@ -70,22 +73,25 @@ class SubscriptionIntraProcess
   using ConstMessageSharedPtr = typename SubscriptionIntraProcessBufferT::ConstDataSharedPtr;
   using MessageUniquePtr = typename SubscriptionIntraProcessBufferT::SubscribedTypeUniquePtr;
   using BufferUniquePtr = typename SubscriptionIntraProcessBufferT::BufferUniquePtr;
+  using StatsHandlerFn = std::function<void(const rmw_message_info_t &, const rclcpp::Time &)>;
 
   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,
+    StatsHandlerFn stats_handler = nullptr)
   : SubscriptionIntraProcessBuffer<SubscribedType, SubscribedTypeAlloc,
       SubscribedTypeDeleter, ROSMessageType>(
       std::make_shared<SubscribedTypeAlloc>(*allocator),
       context,
       topic_name,
       qos_profile,
       buffer_type),
-    any_callback_(callback)
+    any_callback_(callback),
+    stats_handler_(std::move(stats_handler))
   {
     TRACETOOLS_TRACEPOINT(
       rclcpp_subscription_callback_added,
@@ -197,6 +203,19 @@ class SubscriptionIntraProcess
     msg_info.publisher_gid = {0, {0}};
     msg_info.from_intra_process = true;
 
+    std::chrono::time_point<std::chrono::system_clock> now;
+    if (stats_handler_) {
+      RCLCPP_WARN_ONCE(
+        rclcpp::get_logger("rclcpp"),
+        "Intra-process communication does not support accurate message age statistics");
+      now = std::chrono::system_clock::now();
+      // Set source_timestamp to "now" so that message_age reports 0ms rather than
+      // an invalid value taken from an un-initialised timestamp. IPC delivery
+      // has little/no transport latency by definition, so near-zero age is expected.
+      const 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);
 
@@ -208,9 +227,15 @@ class SubscriptionIntraProcess
       any_callback_.dispatch_intra_process(std::move(unique_msg), msg_info);
     }
     shared_ptr.reset();
+
+    if (stats_handler_) {
+      const auto nanos = std::chrono::time_point_cast<std::chrono::nanoseconds>(now);
+      stats_handler_(msg_info, rclcpp::Time(nanos.time_since_epoch().count()));
+    }
   }
 
   AnySubscriptionCallback<MessageT, Alloc> any_callback_;
+  StatsHandlerFn stats_handler_;
 };
 
 }  // namespace experimental

rclcpp/include/rclcpp/subscription.hpp

@@ -164,6 +164,18 @@ class Subscription : public SubscriptionBase
         ROSMessageT,
         AllocatorT>;
 
+      // Build a type-erased stats handler to avoid a circular include chain 
+      // via publisher.hpp and callback_group.hpp
+      typename SubscriptionIntraProcessT::StatsHandlerFn stats_handler = nullptr;
+      if (subscription_topic_statistics) {
+        stats_handler =
+          [subscription_topic_statistics](
+          const rmw_message_info_t & info, const rclcpp::Time & time)
+          {
+            subscription_topic_statistics->handle_message(info, time);
+          };
+      }
+
       // First create a SubscriptionIntraProcess which will be given to the intra-process manager.
       auto context = node_base->get_context();
       subscription_intra_process_ = std::make_shared<SubscriptionIntraProcessT>(
@@ -172,7 +184,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),
+        std::move(stats_handler));
       TRACETOOLS_TRACEPOINT(
         rclcpp_subscription_init,
         static_cast<const void *>(get_subscription_handle().get()),