Integrate topic statistics

rclcpp/include/rclcpp/create_subscription.hpp

@@ -15,15 +15,21 @@
 #ifndef RCLCPP__CREATE_SUBSCRIPTION_HPP_
 #define RCLCPP__CREATE_SUBSCRIPTION_HPP_
 
+#include <chrono>
+#include <functional>
 #include <memory>
 #include <string>
 #include <utility>
 
 #include "rclcpp/node_interfaces/get_node_topics_interface.hpp"
 #include "rclcpp/node_interfaces/node_topics_interface.hpp"
+#include "rclcpp/node_interfaces/node_timers_interface.hpp"
+
 #include "rclcpp/subscription_factory.hpp"
 #include "rclcpp/subscription_options.hpp"
 #include "rclcpp/qos.hpp"
+#include "rclcpp/topic_statistics/subscription_topic_statistics.hpp"
+#include "rclcpp/timer.hpp"
 #include "rmw/qos_profiles.h"
 
 namespace rclcpp
@@ -58,16 +64,49 @@ create_subscription(
   ),
   typename MessageMemoryStrategyT::SharedPtr msg_mem_strat = (
     MessageMemoryStrategyT::create_default()
-  )
+  ),
+  rclcpp::node_interfaces::NodeTimersInterface::SharedPtr node_timer_interface = nullptr
 )
 {
   using rclcpp::node_interfaces::get_node_topics_interface;
   auto node_topics = get_node_topics_interface(std::forward<NodeT>(node));
 
+  std::shared_ptr<rclcpp::topic_statistics::SubscriptionTopicStatistics<CallbackMessageT>>
+  subscription_topic_stats = nullptr;
+
+  if (options.topic_stats_options.state == rclcpp::TopicStatisticsState::Enable) {
+    std::shared_ptr<Publisher<statistics_msgs::msg::MetricsMessage>> publisher =
+      create_publisher<statistics_msgs::msg::MetricsMessage>(
+      node,
+      options.topic_stats_options.publish_topic,
+      qos);
+
+    subscription_topic_stats = std::make_shared<
+      rclcpp::topic_statistics::SubscriptionTopicStatistics<CallbackMessageT>
+      >(node_topics->get_node_base_interface()->get_name(), publisher);
+
+    auto sub_call_back = [subscription_topic_stats]() {
+        subscription_topic_stats->publish_message();
+      };
+
+    auto timer = create_timer(
+      std::chrono::duration_cast<std::chrono::nanoseconds>(
+        options.topic_stats_options.
+        publish_period),
+      sub_call_back,
+      options.callback_group,
+      node_topics->get_node_base_interface(),
+      node_timer_interface
+    );
+
+    subscription_topic_stats->set_publisher_timer(timer);
+  }
+
   auto factory = rclcpp::create_subscription_factory<MessageT>(
     std::forward<CallbackT>(callback),
     options,
-    msg_mem_strat
+    msg_mem_strat,
+    subscription_topic_stats
   );
 
   auto sub = node_topics->create_subscription(topic_name, factory, qos);
@@ -76,6 +115,34 @@ create_subscription(
   return std::dynamic_pointer_cast<SubscriptionT>(sub);
 }
 
+/// Convenience method to create a timer
+/**
+ *
+ * \tparam CallbackT callback template
+ * \param nanos period to exectute callback
+ * \param callback
+ * \param group
+ * \param node_base_
+ * \param node_timers_
+ * \return
+ */
+template<typename CallbackT>
+typename rclcpp::WallTimer<CallbackT>::SharedPtr
+create_timer(
+  std::chrono::nanoseconds nanos,
+  CallbackT callback,
+  rclcpp::callback_group::CallbackGroup::SharedPtr group,
+  rclcpp::node_interfaces::NodeBaseInterface * node_base_,
+  const rclcpp::node_interfaces::NodeTimersInterface::SharedPtr & node_timers_)
+{
+  auto timer = rclcpp::WallTimer<CallbackT>::make_shared(
+    nanos,
+    std::move(callback),
+    node_base_->get_context());
+  node_timers_->add_timer(timer, group);
+  return timer;
+}
+
 }  // namespace rclcpp
 
 #endif  // RCLCPP__CREATE_SUBSCRIPTION_HPP_

rclcpp/include/rclcpp/detail/resolve_enable_topic_statistics.hpp

@@ -42,7 +42,6 @@ resolve_enable_topic_statistics(const OptionsT & options, const NodeBaseT & node
       break;
     default:
       throw std::runtime_error("Unrecognized EnableTopicStatistics value");
-      break;
   }
 
   return topic_stats_enabled;

rclcpp/include/rclcpp/node_impl.hpp

@@ -43,6 +43,7 @@
 #include "rclcpp/qos.hpp"
 #include "rclcpp/type_support_decl.hpp"
 #include "rclcpp/visibility_control.hpp"
+#include "rclcpp/timer.hpp"
 
 #ifndef RCLCPP__NODE_HPP_
 #include "node.hpp"
@@ -98,7 +99,8 @@ Node::create_subscription(
     qos,
     std::forward<CallbackT>(callback),
     options,
-    msg_mem_strat);
+    msg_mem_strat,
+    this->get_node_timers_interface());
 }
 
 template<typename DurationRepT, typename DurationT, typename CallbackT>

rclcpp/include/rclcpp/node_interfaces/node_base.hpp

@@ -43,7 +43,7 @@ class NodeBase : public NodeBaseInterface
     rclcpp::Context::SharedPtr context,
     const rcl_node_options_t & rcl_node_options,
     bool use_intra_process_default,
-    bool enable_topic_statistics_default);
+    bool enable_topic_statistics_default = false);
 
   RCLCPP_PUBLIC
   virtual

rclcpp/include/rclcpp/subscription.hpp

@@ -18,14 +18,14 @@
 #include <rmw/error_handling.h>
 #include <rmw/rmw.h>
 
+#include <chrono>
 #include <functional>
 #include <iostream>
 #include <memory>
 #include <sstream>
 #include <string>
 #include <utility>
 
-
 #include "rcl/error_handling.h"
 #include "rcl/subscription.h"
 
@@ -47,6 +47,7 @@
 #include "rclcpp/type_support_decl.hpp"
 #include "rclcpp/visibility_control.hpp"
 #include "rclcpp/waitable.hpp"
+#include "rclcpp/topic_statistics/subscription_topic_statistics.hpp"
 #include "tracetools/tracetools.h"
 
 namespace rclcpp
@@ -75,6 +76,8 @@ class Subscription : public SubscriptionBase
   using MessageDeleter = allocator::Deleter<MessageAllocator, CallbackMessageT>;
   using ConstMessageSharedPtr = std::shared_ptr<const CallbackMessageT>;
   using MessageUniquePtr = std::unique_ptr<CallbackMessageT, MessageDeleter>;
+  using SubscriptionTopicStatisticsSharedPtr =
+    std::shared_ptr<rclcpp::topic_statistics::SubscriptionTopicStatistics<CallbackMessageT>>;
 
   RCLCPP_SMART_PTR_DEFINITIONS(Subscription)
 
@@ -98,7 +101,8 @@ class Subscription : public SubscriptionBase
     const rclcpp::QoS & qos,
     AnySubscriptionCallback<CallbackMessageT, AllocatorT> callback,
     const rclcpp::SubscriptionOptionsWithAllocator<AllocatorT> & options,
-    typename MessageMemoryStrategyT::SharedPtr message_memory_strategy)
+    typename MessageMemoryStrategyT::SharedPtr message_memory_strategy,
+    SubscriptionTopicStatisticsSharedPtr subscription_topic_statistics = nullptr)
   : SubscriptionBase(
       node_base,
       type_support_handle,
@@ -180,6 +184,10 @@ class Subscription : public SubscriptionBase
       this->setup_intra_process(intra_process_subscription_id, ipm);
     }
 
+    if (subscription_topic_statistics != nullptr) {
+      this->subscription_topic_statistics_ = std::move(subscription_topic_statistics);
+    }
+
     TRACEPOINT(
       rclcpp_subscription_init,
       (const void *)get_subscription_handle().get(),
@@ -260,6 +268,13 @@ class Subscription : public SubscriptionBase
     }
     auto typed_message = std::static_pointer_cast<CallbackMessageT>(message);
     any_callback_.dispatch(typed_message, message_info);
+
+    if (subscription_topic_statistics_) {
+      const auto nanos = std::chrono::time_point_cast<std::chrono::nanoseconds>(
+        std::chrono::steady_clock::now());
+      const auto time = rclcpp::Time(nanos.time_since_epoch().count(), RCL_STEADY_TIME);
+      subscription_topic_statistics_->handle_message(*typed_message, time);
+    }
   }
 
   void
@@ -307,6 +322,8 @@ class Subscription : public SubscriptionBase
   const rclcpp::SubscriptionOptionsWithAllocator<AllocatorT> options_;
   typename message_memory_strategy::MessageMemoryStrategy<CallbackMessageT, AllocatorT>::SharedPtr
     message_memory_strategy_;
+  /// Component which computes and publishes topic statistics for this subscriber
+  SubscriptionTopicStatisticsSharedPtr subscription_topic_statistics_{nullptr};
 };
 
 }  // namespace rclcpp

rclcpp/include/rclcpp/subscription_factory.hpp

@@ -32,6 +32,7 @@
 #include "rclcpp/subscription_options.hpp"
 #include "rclcpp/subscription_traits.hpp"
 #include "rclcpp/visibility_control.hpp"
+#include "rclcpp/topic_statistics/subscription_topic_statistics.hpp"
 
 namespace rclcpp
 {
@@ -78,7 +79,10 @@ SubscriptionFactory
 create_subscription_factory(
   CallbackT && callback,
   const rclcpp::SubscriptionOptionsWithAllocator<AllocatorT> & options,
-  typename MessageMemoryStrategyT::SharedPtr msg_mem_strat)
+  typename MessageMemoryStrategyT::SharedPtr msg_mem_strat,
+  std::shared_ptr<rclcpp::topic_statistics::SubscriptionTopicStatistics<CallbackMessageT>>
+  subscription_topic_stats = nullptr
+)
 {
   auto allocator = options.get_allocator();
 
@@ -88,7 +92,7 @@ create_subscription_factory(
 
   SubscriptionFactory factory {
     // factory function that creates a MessageT specific SubscriptionT
-    [options, msg_mem_strat, any_subscription_callback](
+    [options, msg_mem_strat, any_subscription_callback, subscription_topic_stats](
       rclcpp::node_interfaces::NodeBaseInterface * node_base,
       const std::string & topic_name,
       const rclcpp::QoS & qos
@@ -104,7 +108,8 @@ create_subscription_factory(
         qos,
         any_subscription_callback,
         options,
-        msg_mem_strat);
+        msg_mem_strat,
+        subscription_topic_stats);
       // This is used for setting up things like intra process comms which
       // require this->shared_from_this() which cannot be called from
       // the constructor.