Integrate topic statistics (#1072)

* Add SubscriberTopicStatistics class Signed-off-by: Devin Bonnie <dbbonnie@amazon.com> * Add SubscriberTopicStatistics Test Signed-off-by: Devin Bonnie <dbbonnie@amazon.com> * Address review comments Signed-off-by: Devin Bonnie <dbbonnie@amazon.com> * Modify constructor to allow a node to create necessary components Signed-off-by: Devin Bonnie <dbbonnie@amazon.com> * Fix docstring style Signed-off-by: Devin Bonnie <dbbonnie@amazon.com> * Remove SetPublisherTimer method Signed-off-by: Devin Bonnie <dbbonnie@amazon.com> * Change naming style to match rclcpp Signed-off-by: Devin Bonnie <dbbonnie@amazon.com> * Address style issues Signed-off-by: Devin Bonnie <dbbonnie@amazon.com> * Use rclcpp:Time Signed-off-by: Devin Bonnie <dbbonnie@amazon.com> * Address review comments Signed-off-by: Devin Bonnie <dbbonnie@amazon.com> * Remove unnecessary check for null publisher timer Move anonymous namespace function to private class method Signed-off-by: Devin Bonnie <dbbonnie@amazon.com> * Update message dependency Signed-off-by: Devin Bonnie <dbbonnie@amazon.com> * Initial integration of Subscriber Topic Statistics Signed-off-by: Devin Bonnie <dbbonnie@amazon.com> * Fix nanoseconds used for Topic Stats Signed-off-by: Devin Bonnie <dbbonnie@amazon.com> * Add simple publishing test Minor fixes Signed-off-by: Devin Bonnie <dbbonnie@amazon.com> * Add test utils header Signed-off-by: Devin Bonnie <dbbonnie@amazon.com> * Integrate with Topic Statistics options Fixes after rebasing with master Signed-off-by: Devin Bonnie <dbbonnie@amazon.com> * Update after rebasing Signed-off-by: Devin Bonnie <dbbonnie@amazon.com> * Address minor review comments Signed-off-by: Devin Bonnie <dbbonnie@amazon.com> * Move Topic Statistics instantiation to create_subscription Signed-off-by: Devin Bonnie <dbbonnie@amazon.com> * Fix rebase issue Fix topic statistics enable flag usage Address minor formatting Signed-off-by: Devin Bonnie <dbbonnie@amazon.com> * Move new timer creation method to relevant header Signed-off-by: Devin Bonnie <dbbonnie@amazon.com> * Add timers interface to topic interface Signed-off-by: Devin Bonnie <dbbonnie@amazon.com> * Use new create timer method Signed-off-by: Devin Bonnie <dbbonnie@amazon.com> * Address review comments Signed-off-by: Devin Bonnie <dbbonnie@amazon.com>
2020-04-22 16:09:41 -07:00 · 2020-04-22 16:09:41 -07:00 · bb91b6c2ef
commit bb91b6c2ef
parent 4eab2a3c60
13 changed files with 419 additions and 27 deletions
--- a/rclcpp/include/rclcpp/create_subscription.hpp
+++ b/rclcpp/include/rclcpp/create_subscription.hpp
@ -15,15 +15,25 @@
 #ifndef RCLCPP__CREATE_SUBSCRIPTION_HPP_
 #define RCLCPP__CREATE_SUBSCRIPTION_HPP_

+#include <chrono>
+#include <functional>
 #include <memory>
 #include <string>
 #include <utility>

+#include "rclcpp/detail/resolve_enable_topic_statistics.hpp"
+
+#include "rclcpp/node_interfaces/get_node_timers_interface.hpp"
 #include "rclcpp/node_interfaces/get_node_topics_interface.hpp"
+#include "rclcpp/node_interfaces/node_timers_interface.hpp"
 #include "rclcpp/node_interfaces/node_topics_interface.hpp"
+
+#include "rclcpp/create_timer.hpp"
+#include "rclcpp/qos.hpp"
 #include "rclcpp/subscription_factory.hpp"
 #include "rclcpp/subscription_options.hpp"
-#include "rclcpp/qos.hpp"
+#include "rclcpp/timer.hpp"
+#include "rclcpp/topic_statistics/subscription_topic_statistics.hpp"
 #include "rmw/qos_profiles.h"

 namespace rclcpp
@ -64,10 +74,46 @@ create_subscription(
  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 (rclcpp::detail::resolve_enable_topic_statistics(
+      options,
+      *node_topics->get_node_base_interface()))
+  {
+    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 node_timer_interface = node_topics->get_node_timers_interface();
+
+    auto timer = create_wall_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);
--- a/rclcpp/include/rclcpp/create_timer.hpp
+++ b/rclcpp/include/rclcpp/create_timer.hpp
@ -15,6 +15,8 @@
 #ifndef RCLCPP__CREATE_TIMER_HPP_
 #define RCLCPP__CREATE_TIMER_HPP_

+#include <chrono>
+#include <exception>
 #include <memory>
 #include <string>
 #include <utility>
@ -68,6 +70,46 @@ create_timer(
    group);
 }

+/// Convenience method to create a timer with node resources.
+/**
+ *
+ * \tparam DurationRepT
+ * \tparam DurationT
+ * \tparam CallbackT
+ * \param period period to exectute callback
+ * \param callback callback to execute via the timer period
+ * \param group
+ * \param node_base
+ * \param node_timers
+ * \return
+ * \throws std::invalid argument if either node_base or node_timers
+ * are null
+ */
+template<typename DurationRepT, typename DurationT, typename CallbackT>
+typename rclcpp::WallTimer<CallbackT>::SharedPtr
+create_wall_timer(
+  std::chrono::duration<DurationRepT, DurationT> period,
+  CallbackT callback,
+  rclcpp::callback_group::CallbackGroup::SharedPtr group,
+  node_interfaces::NodeBaseInterface * node_base,
+  node_interfaces::NodeTimersInterface * node_timers)
+{
+  if (node_base == nullptr) {
+    throw std::invalid_argument{"input node_base cannot be null"};
+  }
+
+  if (node_timers == nullptr) {
+    throw std::invalid_argument{"input node_timers cannot be null"};
+  }
+
+  auto timer = rclcpp::WallTimer<CallbackT>::make_shared(
+    std::chrono::duration_cast<std::chrono::nanoseconds>(period),
+    std::move(callback),
+    node_base->get_context());
+  node_timers->add_timer(timer, group);
+  return timer;
+}
+
 }  // namespace rclcpp

 #endif  // RCLCPP__CREATE_TIMER_HPP_
--- a/rclcpp/include/rclcpp/detail/resolve_enable_topic_statistics.hpp
+++ b/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;
--- a/rclcpp/include/rclcpp/node_impl.hpp
+++ b/rclcpp/include/rclcpp/node_impl.hpp
@ -19,6 +19,7 @@
 #include <rmw/rmw.h>

 #include <algorithm>
+#include <chrono>
 #include <cstdlib>
 #include <iostream>
 #include <limits>
@ -37,10 +38,12 @@
 #include "rclcpp/create_client.hpp"
 #include "rclcpp/create_publisher.hpp"
 #include "rclcpp/create_service.hpp"
+#include "rclcpp/create_timer.hpp"
 #include "rclcpp/create_subscription.hpp"
 #include "rclcpp/detail/resolve_enable_topic_statistics.hpp"
 #include "rclcpp/parameter.hpp"
 #include "rclcpp/qos.hpp"
+#include "rclcpp/timer.hpp"
 #include "rclcpp/type_support_decl.hpp"
 #include "rclcpp/visibility_control.hpp"

@ -108,12 +111,12 @@ Node::create_wall_timer(
  CallbackT callback,
  rclcpp::callback_group::CallbackGroup::SharedPtr group)
 {
-  auto timer = rclcpp::WallTimer<CallbackT>::make_shared(
-    std::chrono::duration_cast<std::chrono::nanoseconds>(period),
+  return rclcpp::create_wall_timer(
+    period,
    std::move(callback),
-    this->node_base_->get_context());
-  node_timers_->add_timer(timer, group);
-  return timer;
+    group,
+    this->node_base_.get(),
+    this->node_timers_.get());
 }

 template<typename ServiceT>
--- a/rclcpp/include/rclcpp/node_interfaces/node_topics.hpp
+++ b/rclcpp/include/rclcpp/node_interfaces/node_topics.hpp
@ -22,6 +22,7 @@

 #include "rclcpp/macros.hpp"
 #include "rclcpp/node_interfaces/node_base_interface.hpp"
+#include "rclcpp/node_interfaces/node_timers_interface.hpp"
 #include "rclcpp/node_interfaces/node_topics_interface.hpp"
 #include "rclcpp/publisher.hpp"
 #include "rclcpp/publisher_factory.hpp"
@ -39,7 +40,9 @@ public:
  RCLCPP_SMART_PTR_ALIASES_ONLY(NodeTopicsInterface)

  RCLCPP_PUBLIC
-  explicit NodeTopics(rclcpp::node_interfaces::NodeBaseInterface * node_base);
+  NodeTopics(
+    rclcpp::node_interfaces::NodeBaseInterface * node_base,
+    rclcpp::node_interfaces::NodeTimersInterface * node_timers);

  RCLCPP_PUBLIC
  ~NodeTopics() override;
@ -74,10 +77,15 @@ public:
  rclcpp::node_interfaces::NodeBaseInterface *
  get_node_base_interface() const override;

+  RCLCPP_PUBLIC
+  rclcpp::node_interfaces::NodeTimersInterface *
+  get_node_timers_interface() const override;
+
 private:
  RCLCPP_DISABLE_COPY(NodeTopics)

  rclcpp::node_interfaces::NodeBaseInterface * node_base_;
+  rclcpp::node_interfaces::NodeTimersInterface * node_timers_;
 };

 }  // namespace node_interfaces
--- a/rclcpp/include/rclcpp/node_interfaces/node_topics_interface.hpp
+++ b/rclcpp/include/rclcpp/node_interfaces/node_topics_interface.hpp
@ -25,6 +25,7 @@
 #include "rclcpp/callback_group.hpp"
 #include "rclcpp/macros.hpp"
 #include "rclcpp/node_interfaces/node_base_interface.hpp"
+#include "rclcpp/node_interfaces/node_timers_interface.hpp"
 #include "rclcpp/publisher.hpp"
 #include "rclcpp/publisher_factory.hpp"
 #include "rclcpp/subscription.hpp"
@ -80,6 +81,11 @@ public:
  virtual
  rclcpp::node_interfaces::NodeBaseInterface *
  get_node_base_interface() const = 0;
+
+  RCLCPP_PUBLIC
+  virtual
+  rclcpp::node_interfaces::NodeTimersInterface *
+  get_node_timers_interface() const = 0;
 };

 }  // namespace node_interfaces
--- a/rclcpp/include/rclcpp/subscription.hpp
+++ b/rclcpp/include/rclcpp/subscription.hpp
@ -18,6 +18,7 @@
 #include <rmw/error_handling.h>
 #include <rmw/rmw.h>

+#include <chrono>
 #include <functional>
 #include <iostream>
 #include <memory>
@ -25,7 +26,6 @@
 #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 @@ public:
  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 @@ public:
    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 @@ public:
      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 @@ public:
    }
    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 @@ private:
  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
--- a/rclcpp/include/rclcpp/subscription_factory.hpp
+++ b/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.