From 538fbcb314a03bffcddae90fd52d7082561fb20c Mon Sep 17 00:00:00 2001 From: Devin Bonnie Date: Mon, 6 Apr 2020 16:31:50 -0700 Subject: [PATCH 01/25] Add SubscriberTopicStatistics class Signed-off-by: Devin Bonnie --- .../subscriber_topic_statistics.hpp | 178 ++++++++++++++++++ 1 file changed, 178 insertions(+) create mode 100644 rclcpp/include/rclcpp/topic_statistics/subscriber_topic_statistics.hpp diff --git a/rclcpp/include/rclcpp/topic_statistics/subscriber_topic_statistics.hpp b/rclcpp/include/rclcpp/topic_statistics/subscriber_topic_statistics.hpp new file mode 100644 index 0000000000..2ebf761fa6 --- /dev/null +++ b/rclcpp/include/rclcpp/topic_statistics/subscriber_topic_statistics.hpp @@ -0,0 +1,178 @@ +// Copyright 2020 Amazon.com, Inc. or its affiliates. All Rights Reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLCPP__SUBSCRIBER_TOPIC_STATISTICS_HPP_ +#define RCLCPP__SUBSCRIBER_TOPIC_STATISTICS_HPP_ + +#include +#include + +#include "rcl/time.h" +#include "rclcpp/time.hpp" +#include "rclcpp/publisher.hpp" +#include "rclcpp/timer.hpp" + +#include "metrics_statistics_msgs/msg/metrics_message.hpp" + +#include "libstatistics_collector/topic_statistics_collector/constants.hpp" +#include "libstatistics_collector/topic_statistics_collector/received_message_age.hpp" +#include "libstatistics_collector/topic_statistics_collector/received_message_period.hpp" + + +namespace rclcpp { +namespace topic_statistics { + +using libstatistics_collector::collector::GenerateStatisticMessage; +using metrics_statistics_msgs::msg::MetricsMessage; +using metrics_statistics_msgs::msg::StatisticDataPoint; +using metrics_statistics_msgs::msg::StatisticDataType; + +/** + * Class used to collect, measure, and publish topic statistics data. Current statistics + * supported for subscribers are received message age and received message period. + * + * @tparam CallbackMessageT the subscribed message type + */ +template +class SubscriberTopicStatistics { + +using topic_stats_collector = + topic_statistics_collector::TopicStatisticsCollector; +using received_message_age = + topic_statistics_collector::ReceivedMessageAgeCollector; +using received_message_period = + topic_statistics_collector::ReceivedMessagePeriodCollector; + +public: + /// Construct a SubcriberTopicStatistics object. + /** + * This object wraps utilities, defined in libstatistics_collector, to collect, + * measure, and publish topic statistics data. + * + * @param node_name the name of the node, which created this instance, in order to denote + * topic source + * @param publisher instance constructed by the node in order to publish statistics data + */ + SubscriberTopicStatistics(const std::string & node_name, + const rclcpp::Publisher::SharedPtr & publisher) + : node_name_(node_name), + publisher_(std::move(publisher)) + { + const auto rma = std::make_unique(); + rma->Start(); + subscriber_statistics_collectors_.emplace_back(std::move(rma)); + + const auto rmp = std::make_unique(); + rmp->Start(); + subscriber_statistics_collectors_.emplace_back(std::move(rmp)); + + window_start_ = rclcpp::Time(GetCurrentNanosecondsSinceEpoch()); + } + + virtual ~SubscriberTopicStatistics() + { + TearDown(); + } + + /// Stop all collectors, clear measurements, stop publishing timer, and reset publisher. + virtual void TearDown() + { + for (auto & collector: subscriber_statistics_collectors_) { + collector->Stop(); + } + + subscriber_statistics_collectors_.clear(); + + if (publisher_timer_) { + publisher_timer_->cancel(); + publisher_timer_.reset(); + } + if (publisher_) { + publisher_.reset(); + } + } + + /// Set the timer used to publish statistics messages. + /** + * @param measurement_timer the timer to fire the publisher, created by the node + */ + void SetPublisherTimer(const rclcpp::TimerBase::SharedPtr & publisher_timer) + { + publisher_timer_ = std::move(publisher_timer); + } + + /// Handle a message received by the subscription to collect statistics. + /** + * @param received_message the message received by the subscription + * @param now_nanoseconds current time in nanoseconds + */ + virtual void OnMessageReceived( + const CallbackMessageT & received_message, + const rcl_time_point_value_t now_nanoseconds) const + { + (void) received_message; + + for (auto & collector: subscriber_statistics_collectors_) { + collector->OnMessageReceived(received_message, now_nanoseconds); + } + } + +private: + /// Publish a populated MetricsStatisticsMessage + virtual void PublishMessage() + { + rclcpp::Time window_end{GetCurrentNanosecondsSinceEpoch()}; + + for (auto & collector: subscriber_statistics_collectors_) { + const auto collected_stats = collector->GetStatisticsResults(); + + auto message = libstatistics_collector::collector::GenerateStatisticMessage( + node_name_, + collector->GetMetricName(), + collector->GetMetricUnit(), + window_start_, + window_end, + collected_stats); + publisher_->publish(message); + } + window_start_ = window_end; + } + + ///Return the current nanoseconds (count) since epoch + /** + * Based on design discussions, using harccoded time instead of a node's clock + * due to lifecycle issues. + * @return the current nanoseconds (count) since epoch + */ + uint64_t GetCurrentNanosecondsSinceEpoch() + { + const auto now = std::chrono::system_clock::now(); + return std::chrono::duration_cast(now.time_since_epoch()).count(); + } + + /// Collection of statistics collectors + std::vector> subscriber_statistics_collectors_{}; + /// Node name used to generate topic statistics messages to be published + const std::string node_name_; + /// Publisher, created by the node, used to publish topic statistics messages + rclcpp::Publisher::SharedPtr publisher_{nullptr}; + /// Timer which fires the publisher + rclcpp::TimerBase::SharedPtr publisher_timer{nullptr}; + /// The start of the collection window, used in the published topic statistics message + rclcpp::Time window_start_; +}; +} // namespace topic_statistics +} // namespace rclcpp + +#endif //RCLCPP__SUBSCRIBER_TOPIC_STATISTICS_HPP_ From a61ad418c4a2c6c2800f28042e536b92b7d691f3 Mon Sep 17 00:00:00 2001 From: Devin Bonnie Date: Tue, 7 Apr 2020 16:47:01 -0700 Subject: [PATCH 02/25] Add SubscriberTopicStatistics Test Signed-off-by: Devin Bonnie --- rclcpp/CMakeLists.txt | 1 + .../subscriber_topic_statistics.hpp | 74 ++++++----- .../test_subscriber_topic_statistics.cpp | 115 ++++++++++++++++++ 3 files changed, 161 insertions(+), 29 deletions(-) create mode 100644 rclcpp/test/topic_statistics/test_subscriber_topic_statistics.cpp diff --git a/rclcpp/CMakeLists.txt b/rclcpp/CMakeLists.txt index 3918120e5a..9d5ff91f94 100644 --- a/rclcpp/CMakeLists.txt +++ b/rclcpp/CMakeLists.txt @@ -5,6 +5,7 @@ project(rclcpp) find_package(ament_cmake_ros REQUIRED) find_package(builtin_interfaces REQUIRED) find_package(libstatistics_collector REQUIRED) +find_package(metrics_statistics_msgs REQUIRED) find_package(rcl REQUIRED) find_package(rcl_interfaces REQUIRED) find_package(rcl_yaml_param_parser REQUIRED) diff --git a/rclcpp/include/rclcpp/topic_statistics/subscriber_topic_statistics.hpp b/rclcpp/include/rclcpp/topic_statistics/subscriber_topic_statistics.hpp index 2ebf761fa6..50147d1b44 100644 --- a/rclcpp/include/rclcpp/topic_statistics/subscriber_topic_statistics.hpp +++ b/rclcpp/include/rclcpp/topic_statistics/subscriber_topic_statistics.hpp @@ -12,11 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef RCLCPP__SUBSCRIBER_TOPIC_STATISTICS_HPP_ -#define RCLCPP__SUBSCRIBER_TOPIC_STATISTICS_HPP_ +#ifndef RCLCPP__TOPIC_STATISTICS__SUBSCRIBER_TOPIC_STATISTICS_HPP_ +#define RCLCPP__TOPIC_STATISTICS__SUBSCRIBER_TOPIC_STATISTICS_HPP_ -#include +#include #include +#include +#include #include "rcl/time.h" #include "rclcpp/time.hpp" @@ -25,18 +27,20 @@ #include "metrics_statistics_msgs/msg/metrics_message.hpp" +#include "libstatistics_collector/collector/generate_statistics_message.hpp" #include "libstatistics_collector/topic_statistics_collector/constants.hpp" -#include "libstatistics_collector/topic_statistics_collector/received_message_age.hpp" #include "libstatistics_collector/topic_statistics_collector/received_message_period.hpp" +#include "libstatistics_collector/moving_average_statistics/types.hpp" -namespace rclcpp { -namespace topic_statistics { +namespace rclcpp +{ +namespace topic_statistics +{ using libstatistics_collector::collector::GenerateStatisticMessage; using metrics_statistics_msgs::msg::MetricsMessage; -using metrics_statistics_msgs::msg::StatisticDataPoint; -using metrics_statistics_msgs::msg::StatisticDataType; +using libstatistics_collector::moving_average_statistics::StatisticData; /** * Class used to collect, measure, and publish topic statistics data. Current statistics @@ -45,14 +49,14 @@ using metrics_statistics_msgs::msg::StatisticDataType; * @tparam CallbackMessageT the subscribed message type */ template -class SubscriberTopicStatistics { - -using topic_stats_collector = - topic_statistics_collector::TopicStatisticsCollector; -using received_message_age = - topic_statistics_collector::ReceivedMessageAgeCollector; -using received_message_period = - topic_statistics_collector::ReceivedMessagePeriodCollector; +class SubscriberTopicStatistics +{ + using topic_stats_collector = + libstatistics_collector::topic_statistics_collector::TopicStatisticsCollector< + CallbackMessageT>; + using received_message_period = + libstatistics_collector::topic_statistics_collector::ReceivedMessagePeriodCollector< + CallbackMessageT>; public: /// Construct a SubcriberTopicStatistics object. @@ -64,16 +68,15 @@ using received_message_period = * topic source * @param publisher instance constructed by the node in order to publish statistics data */ - SubscriberTopicStatistics(const std::string & node_name, + SubscriberTopicStatistics( + const std::string & node_name, const rclcpp::Publisher::SharedPtr & publisher) - : node_name_(node_name), + : node_name_(node_name), publisher_(std::move(publisher)) { - const auto rma = std::make_unique(); - rma->Start(); - subscriber_statistics_collectors_.emplace_back(std::move(rma)); + // TODO(dbbonnie): ros-tooling/aws-roadmap/issues/226, received message age - const auto rmp = std::make_unique(); + auto rmp = std::make_unique(); rmp->Start(); subscriber_statistics_collectors_.emplace_back(std::move(rmp)); @@ -88,7 +91,7 @@ using received_message_period = /// Stop all collectors, clear measurements, stop publishing timer, and reset publisher. virtual void TearDown() { - for (auto & collector: subscriber_statistics_collectors_) { + for (auto & collector : subscriber_statistics_collectors_) { collector->Stop(); } @@ -123,18 +126,31 @@ using received_message_period = { (void) received_message; - for (auto & collector: subscriber_statistics_collectors_) { + for (auto & collector : subscriber_statistics_collectors_) { collector->OnMessageReceived(received_message, now_nanoseconds); } } + /// Return a vector of all the currently collected data + /** + * @return a vector of all the collected data + */ + std::vector GetCurrentCollectorData() const + { + std::vector data; + for (const auto & collector : subscriber_statistics_collectors_) { + data.push_back(collector->GetStatisticsResults()); + } + return data; + } + private: /// Publish a populated MetricsStatisticsMessage virtual void PublishMessage() { rclcpp::Time window_end{GetCurrentNanosecondsSinceEpoch()}; - for (auto & collector: subscriber_statistics_collectors_) { + for (auto & collector : subscriber_statistics_collectors_) { const auto collected_stats = collector->GetStatisticsResults(); auto message = libstatistics_collector::collector::GenerateStatisticMessage( @@ -149,13 +165,13 @@ using received_message_period = window_start_ = window_end; } - ///Return the current nanoseconds (count) since epoch + /// Return the current nanoseconds (count) since epoch /** * Based on design discussions, using harccoded time instead of a node's clock * due to lifecycle issues. * @return the current nanoseconds (count) since epoch */ - uint64_t GetCurrentNanosecondsSinceEpoch() + int64_t GetCurrentNanosecondsSinceEpoch() { const auto now = std::chrono::system_clock::now(); return std::chrono::duration_cast(now.time_since_epoch()).count(); @@ -168,11 +184,11 @@ using received_message_period = /// Publisher, created by the node, used to publish topic statistics messages rclcpp::Publisher::SharedPtr publisher_{nullptr}; /// Timer which fires the publisher - rclcpp::TimerBase::SharedPtr publisher_timer{nullptr}; + rclcpp::TimerBase::SharedPtr publisher_timer_{nullptr}; /// The start of the collection window, used in the published topic statistics message rclcpp::Time window_start_; }; } // namespace topic_statistics } // namespace rclcpp -#endif //RCLCPP__SUBSCRIBER_TOPIC_STATISTICS_HPP_ +#endif // RCLCPP__TOPIC_STATISTICS__SUBSCRIBER_TOPIC_STATISTICS_HPP_ diff --git a/rclcpp/test/topic_statistics/test_subscriber_topic_statistics.cpp b/rclcpp/test/topic_statistics/test_subscriber_topic_statistics.cpp new file mode 100644 index 0000000000..e69026b5c7 --- /dev/null +++ b/rclcpp/test/topic_statistics/test_subscriber_topic_statistics.cpp @@ -0,0 +1,115 @@ +// Copyright 2020 Amazon.com, Inc. or its affiliates. All Rights Reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include + +#include "libstatistics_collector/moving_average_statistics/types.hpp" + +#include "metrics_statistics_msgs/msg/metrics_message.hpp" + +#include "rclcpp/create_publisher.hpp" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp/topic_statistics/subscriber_topic_statistics.hpp" + +#include "test_msgs/msg/empty.hpp" + + +namespace +{ +constexpr const char kTestNodeName[]{"test_sub_stats_node"}; +constexpr const char kTestSubStatsTopic[]{"/test_sub_stats_topic"}; +constexpr const char kTestTopicStatisticsTopic[]{"/test_topic_statistics_topic"}; +constexpr const uint16_t kNoSamples = 0; +} // namespace + +using test_msgs::msg::Empty; +using metrics_statistics_msgs::msg::MetricsMessage; +using rclcpp::topic_statistics::SubscriberTopicStatistics; + +/** + * Empty subscriber node: used to create subscriber topic statistics requirements + */ +class EmptySubscriber : public rclcpp::Node +{ +public: + EmptySubscriber(const std::string & name, const std::string & topic) + : Node(name) + { + auto callback = [this](Empty::UniquePtr msg) { + this->ReceiveMessage(*msg); + }; + subscription_ = create_subscription>( + topic, + 0 /*history_depth*/, + callback); + } + +private: + void ReceiveMessage(const Empty & msg) const + { + (void) msg; + } + + rclcpp::Subscription::SharedPtr subscription_; +}; + +/** + * Test fixture to bring up and teardown rclcpp + */ +class TestSubscriberTopicStatisticsFixture : public ::testing::Test +{ +protected: + void SetUp() + { + rclcpp::init(0, nullptr); + } + + void TearDown() + { + rclcpp::shutdown(); + } +}; + +TEST_F(TestSubscriberTopicStatisticsFixture, TestManualConstruction) +{ + auto empty_subscriber = std::make_shared( + kTestNodeName, + kTestSubStatsTopic); + + // manually create publisher tied to the node + auto topic_stats_publisher = + empty_subscriber->create_publisher( + kTestTopicStatisticsTopic, + 10); + + // construct the instance + auto sub_topic_stats = std::make_unique>( + empty_subscriber->get_name(), + topic_stats_publisher); + + using libstatistics_collector::moving_average_statistics::StatisticData; + + // expect no data has been collected / no samples received + for (const auto & data : sub_topic_stats->GetCurrentCollectorData()) { + EXPECT_TRUE(std::isnan(data.average)); + EXPECT_TRUE(std::isnan(data.min)); + EXPECT_TRUE(std::isnan(data.max)); + EXPECT_TRUE(std::isnan(data.standard_deviation)); + EXPECT_EQ(kNoSamples, data.sample_count); + } +} From fe4fad9a87d25bb3ffcbe88c96b686cacd19caf7 Mon Sep 17 00:00:00 2001 From: Devin Bonnie Date: Wed, 8 Apr 2020 13:47:38 -0700 Subject: [PATCH 03/25] Address review comments Signed-off-by: Devin Bonnie --- .../subscriber_topic_statistics.hpp | 75 ++++++++++--------- .../test_subscriber_topic_statistics.cpp | 11 +-- 2 files changed, 44 insertions(+), 42 deletions(-) diff --git a/rclcpp/include/rclcpp/topic_statistics/subscriber_topic_statistics.hpp b/rclcpp/include/rclcpp/topic_statistics/subscriber_topic_statistics.hpp index 50147d1b44..660b9667c2 100644 --- a/rclcpp/include/rclcpp/topic_statistics/subscriber_topic_statistics.hpp +++ b/rclcpp/include/rclcpp/topic_statistics/subscriber_topic_statistics.hpp @@ -32,6 +32,20 @@ #include "libstatistics_collector/topic_statistics_collector/received_message_period.hpp" #include "libstatistics_collector/moving_average_statistics/types.hpp" +namespace +{ +/// Return the current nanoseconds (count) since epoch +/** + * For now, use hard coded time instead of a node's clock (to support sim time and playback) + * due to node clock lifecycle issues. + * @return the current nanoseconds (count) since epoch + */ +int64_t GetCurrentNanosecondsSinceEpoch() +{ + const auto now = std::chrono::system_clock::now(); + return std::chrono::duration_cast(now.time_since_epoch()).count(); +} +} // namespace namespace rclcpp { @@ -51,10 +65,10 @@ using libstatistics_collector::moving_average_statistics::StatisticData; template class SubscriberTopicStatistics { - using topic_stats_collector = + using TopicStatsCollector = libstatistics_collector::topic_statistics_collector::TopicStatisticsCollector< CallbackMessageT>; - using received_message_period = + using ReceivedMessagePeriod = libstatistics_collector::topic_statistics_collector::ReceivedMessagePeriodCollector< CallbackMessageT>; @@ -76,9 +90,9 @@ class SubscriberTopicStatistics { // TODO(dbbonnie): ros-tooling/aws-roadmap/issues/226, received message age - auto rmp = std::make_unique(); - rmp->Start(); - subscriber_statistics_collectors_.emplace_back(std::move(rmp)); + auto received_message_period = std::make_unique(); + received_message_period->Start(); + subscriber_statistics_collectors_.emplace_back(std::move(received_message_period)); window_start_ = rclcpp::Time(GetCurrentNanosecondsSinceEpoch()); } @@ -88,24 +102,6 @@ class SubscriberTopicStatistics TearDown(); } - /// Stop all collectors, clear measurements, stop publishing timer, and reset publisher. - virtual void TearDown() - { - for (auto & collector : subscriber_statistics_collectors_) { - collector->Stop(); - } - - subscriber_statistics_collectors_.clear(); - - if (publisher_timer_) { - publisher_timer_->cancel(); - publisher_timer_.reset(); - } - if (publisher_) { - publisher_.reset(); - } - } - /// Set the timer used to publish statistics messages. /** * @param measurement_timer the timer to fire the publisher, created by the node @@ -126,7 +122,7 @@ class SubscriberTopicStatistics { (void) received_message; - for (auto & collector : subscriber_statistics_collectors_) { + for (const auto & collector : subscriber_statistics_collectors_) { collector->OnMessageReceived(received_message, now_nanoseconds); } } @@ -145,6 +141,23 @@ class SubscriberTopicStatistics } private: + /// Stop all collectors, clear measurements, stop publishing timer, and reset publisher. + virtual void TearDown() + { + for (auto & collector : subscriber_statistics_collectors_) { + collector->Stop(); + } + + subscriber_statistics_collectors_.clear(); + + if (publisher_timer_) { + publisher_timer_->cancel(); + publisher_timer_.reset(); + } + + publisher_.reset(); + } + /// Publish a populated MetricsStatisticsMessage virtual void PublishMessage() { @@ -165,20 +178,8 @@ class SubscriberTopicStatistics window_start_ = window_end; } - /// Return the current nanoseconds (count) since epoch - /** - * Based on design discussions, using harccoded time instead of a node's clock - * due to lifecycle issues. - * @return the current nanoseconds (count) since epoch - */ - int64_t GetCurrentNanosecondsSinceEpoch() - { - const auto now = std::chrono::system_clock::now(); - return std::chrono::duration_cast(now.time_since_epoch()).count(); - } - /// Collection of statistics collectors - std::vector> subscriber_statistics_collectors_{}; + std::vector> subscriber_statistics_collectors_{}; /// Node name used to generate topic statistics messages to be published const std::string node_name_; /// Publisher, created by the node, used to publish topic statistics messages diff --git a/rclcpp/test/topic_statistics/test_subscriber_topic_statistics.cpp b/rclcpp/test/topic_statistics/test_subscriber_topic_statistics.cpp index e69026b5c7..c17106ea1d 100644 --- a/rclcpp/test/topic_statistics/test_subscriber_topic_statistics.cpp +++ b/rclcpp/test/topic_statistics/test_subscriber_topic_statistics.cpp @@ -23,6 +23,8 @@ #include "rclcpp/create_publisher.hpp" #include "rclcpp/rclcpp.hpp" +#include "rclcpp/qos.hpp" + #include "rclcpp/topic_statistics/subscriber_topic_statistics.hpp" #include "test_msgs/msg/empty.hpp" @@ -33,7 +35,7 @@ namespace constexpr const char kTestNodeName[]{"test_sub_stats_node"}; constexpr const char kTestSubStatsTopic[]{"/test_sub_stats_topic"}; constexpr const char kTestTopicStatisticsTopic[]{"/test_topic_statistics_topic"}; -constexpr const uint16_t kNoSamples = 0; +constexpr const uint64_t kNoSamples{0}; } // namespace using test_msgs::msg::Empty; @@ -55,14 +57,13 @@ class EmptySubscriber : public rclcpp::Node subscription_ = create_subscription>( topic, - 0 /*history_depth*/, + rclcpp::QoS(rclcpp::KeepAll()), callback); } private: - void ReceiveMessage(const Empty & msg) const + void ReceiveMessage(const Empty &) const { - (void) msg; } rclcpp::Subscription::SharedPtr subscription_; @@ -76,7 +77,7 @@ class TestSubscriberTopicStatisticsFixture : public ::testing::Test protected: void SetUp() { - rclcpp::init(0, nullptr); + rclcpp::init(0 /* argc */, nullptr /* argv */); } void TearDown() From fdb6c64c82d5874223eb583ee2c503abc44a00df Mon Sep 17 00:00:00 2001 From: Devin Bonnie Date: Mon, 13 Apr 2020 14:24:27 -0700 Subject: [PATCH 04/25] Modify constructor to allow a node to create necessary components Signed-off-by: Devin Bonnie --- .../subscriber_topic_statistics.hpp | 64 +++++++++++++------ .../test_subscriber_topic_statistics.cpp | 22 +++---- 2 files changed, 52 insertions(+), 34 deletions(-) diff --git a/rclcpp/include/rclcpp/topic_statistics/subscriber_topic_statistics.hpp b/rclcpp/include/rclcpp/topic_statistics/subscriber_topic_statistics.hpp index 660b9667c2..79c8b9b14f 100644 --- a/rclcpp/include/rclcpp/topic_statistics/subscriber_topic_statistics.hpp +++ b/rclcpp/include/rclcpp/topic_statistics/subscriber_topic_statistics.hpp @@ -20,7 +20,10 @@ #include #include + +#include "rclcpp/create_publisher.hpp" #include "rcl/time.h" +#include "rclcpp/node.hpp" #include "rclcpp/time.hpp" #include "rclcpp/publisher.hpp" #include "rclcpp/timer.hpp" @@ -34,7 +37,7 @@ namespace { -/// Return the current nanoseconds (count) since epoch +/// Return the current nanoseconds (count) since epoch. /** * For now, use hard coded time instead of a node's clock (to support sim time and playback) * due to node clock lifecycle issues. @@ -52,6 +55,9 @@ namespace rclcpp namespace topic_statistics { +constexpr const char kDefaultPublishTopicName[]{"topic_statistics"}; +constexpr const std::chrono::milliseconds kDefaultPublishingPeriod{std::chrono::minutes(1)}; + using libstatistics_collector::collector::GenerateStatisticMessage; using metrics_statistics_msgs::msg::MetricsMessage; using libstatistics_collector::moving_average_statistics::StatisticData; @@ -75,26 +81,34 @@ class SubscriberTopicStatistics public: /// Construct a SubcriberTopicStatistics object. /** - * This object wraps utilities, defined in libstatistics_collector, to collect, - * measure, and publish topic statistics data. + * This object wraps utilities, defined in libstatistics_collector, to collect, + * measure, and publish topic statistics data. * - * @param node_name the name of the node, which created this instance, in order to denote - * topic source - * @param publisher instance constructed by the node in order to publish statistics data - */ + * @param node the node creating the subscription, used to create the publisher and timer to + * publish topic statistics. + * @param publishing_topic the topic to publish statistics to + * @param publishing_period the period at which topic statistics messages are published + */ SubscriberTopicStatistics( - const std::string & node_name, - const rclcpp::Publisher::SharedPtr & publisher) - : node_name_(node_name), - publisher_(std::move(publisher)) + rclcpp::Node & node, + const std::string & publishing_topic = kDefaultPublishTopicName, + const std::chrono::milliseconds & publishing_period = kDefaultPublishingPeriod) { - // TODO(dbbonnie): ros-tooling/aws-roadmap/issues/226, received message age + publisher_ = + node.create_publisher( + publishing_topic, + 10); - auto received_message_period = std::make_unique(); - received_message_period->Start(); - subscriber_statistics_collectors_.emplace_back(std::move(received_message_period)); + auto callback = [this]() + { + this->PublishMessage(); + }; - window_start_ = rclcpp::Time(GetCurrentNanosecondsSinceEpoch()); + publisher_timer_ = node.create_wall_timer(publishing_period, callback); + + node_name_ = node.get_name(); + + BringUp(); } virtual ~SubscriberTopicStatistics() @@ -141,8 +155,18 @@ class SubscriberTopicStatistics } private: + /// Construct and start all collectors and set window_start_. + void BringUp() + { + auto received_message_period = std::make_unique(); + received_message_period->Start(); + subscriber_statistics_collectors_.emplace_back(std::move(received_message_period)); + + window_start_ = rclcpp::Time(GetCurrentNanosecondsSinceEpoch()); + } + /// Stop all collectors, clear measurements, stop publishing timer, and reset publisher. - virtual void TearDown() + void TearDown() { for (auto & collector : subscriber_statistics_collectors_) { collector->Stop(); @@ -181,11 +205,11 @@ class SubscriberTopicStatistics /// Collection of statistics collectors std::vector> subscriber_statistics_collectors_{}; /// Node name used to generate topic statistics messages to be published - const std::string node_name_; + std::string node_name_; /// Publisher, created by the node, used to publish topic statistics messages - rclcpp::Publisher::SharedPtr publisher_{nullptr}; + rclcpp::Publisher::SharedPtr publisher_; /// Timer which fires the publisher - rclcpp::TimerBase::SharedPtr publisher_timer_{nullptr}; + rclcpp::TimerBase::SharedPtr publisher_timer_; /// The start of the collection window, used in the published topic statistics message rclcpp::Time window_start_; }; diff --git a/rclcpp/test/topic_statistics/test_subscriber_topic_statistics.cpp b/rclcpp/test/topic_statistics/test_subscriber_topic_statistics.cpp index c17106ea1d..03f92a7935 100644 --- a/rclcpp/test/topic_statistics/test_subscriber_topic_statistics.cpp +++ b/rclcpp/test/topic_statistics/test_subscriber_topic_statistics.cpp @@ -24,6 +24,7 @@ #include "rclcpp/create_publisher.hpp" #include "rclcpp/rclcpp.hpp" #include "rclcpp/qos.hpp" +#include "rclcpp/node.hpp" #include "rclcpp/topic_statistics/subscriber_topic_statistics.hpp" @@ -41,6 +42,7 @@ constexpr const uint64_t kNoSamples{0}; using test_msgs::msg::Empty; using metrics_statistics_msgs::msg::MetricsMessage; using rclcpp::topic_statistics::SubscriberTopicStatistics; +using libstatistics_collector::moving_average_statistics::StatisticData; /** * Empty subscriber node: used to create subscriber topic statistics requirements @@ -78,32 +80,24 @@ class TestSubscriberTopicStatisticsFixture : public ::testing::Test void SetUp() { rclcpp::init(0 /* argc */, nullptr /* argv */); + empty_subscriber = std::make_shared( + kTestNodeName, + kTestSubStatsTopic); } void TearDown() { rclcpp::shutdown(); + empty_subscriber.reset(); } + std::shared_ptr empty_subscriber; }; TEST_F(TestSubscriberTopicStatisticsFixture, TestManualConstruction) { - auto empty_subscriber = std::make_shared( - kTestNodeName, - kTestSubStatsTopic); - - // manually create publisher tied to the node - auto topic_stats_publisher = - empty_subscriber->create_publisher( - kTestTopicStatisticsTopic, - 10); - // construct the instance auto sub_topic_stats = std::make_unique>( - empty_subscriber->get_name(), - topic_stats_publisher); - - using libstatistics_collector::moving_average_statistics::StatisticData; + *empty_subscriber); // expect no data has been collected / no samples received for (const auto & data : sub_topic_stats->GetCurrentCollectorData()) { From 043b9a9cea690e632cb6aafc1d289ae5a3ca0770 Mon Sep 17 00:00:00 2001 From: Devin Bonnie Date: Mon, 13 Apr 2020 14:30:49 -0700 Subject: [PATCH 05/25] Fix docstring style Signed-off-by: Devin Bonnie --- .../subscriber_topic_statistics.hpp | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/rclcpp/include/rclcpp/topic_statistics/subscriber_topic_statistics.hpp b/rclcpp/include/rclcpp/topic_statistics/subscriber_topic_statistics.hpp index 79c8b9b14f..9d1cbb36a3 100644 --- a/rclcpp/include/rclcpp/topic_statistics/subscriber_topic_statistics.hpp +++ b/rclcpp/include/rclcpp/topic_statistics/subscriber_topic_statistics.hpp @@ -41,7 +41,7 @@ namespace /** * For now, use hard coded time instead of a node's clock (to support sim time and playback) * due to node clock lifecycle issues. - * @return the current nanoseconds (count) since epoch + * \return the current nanoseconds (count) since epoch */ int64_t GetCurrentNanosecondsSinceEpoch() { @@ -66,7 +66,7 @@ using libstatistics_collector::moving_average_statistics::StatisticData; * Class used to collect, measure, and publish topic statistics data. Current statistics * supported for subscribers are received message age and received message period. * - * @tparam CallbackMessageT the subscribed message type + * \tparam CallbackMessageT the subscribed message type */ template class SubscriberTopicStatistics @@ -84,10 +84,10 @@ class SubscriberTopicStatistics * This object wraps utilities, defined in libstatistics_collector, to collect, * measure, and publish topic statistics data. * - * @param node the node creating the subscription, used to create the publisher and timer to + * \param node the node creating the subscription, used to create the publisher and timer to * publish topic statistics. - * @param publishing_topic the topic to publish statistics to - * @param publishing_period the period at which topic statistics messages are published + * \param publishing_topic the topic to publish statistics to + * \param publishing_period the period at which topic statistics messages are published */ SubscriberTopicStatistics( rclcpp::Node & node, @@ -118,7 +118,7 @@ class SubscriberTopicStatistics /// Set the timer used to publish statistics messages. /** - * @param measurement_timer the timer to fire the publisher, created by the node + * \param measurement_timer the timer to fire the publisher, created by the node */ void SetPublisherTimer(const rclcpp::TimerBase::SharedPtr & publisher_timer) { @@ -127,8 +127,8 @@ class SubscriberTopicStatistics /// Handle a message received by the subscription to collect statistics. /** - * @param received_message the message received by the subscription - * @param now_nanoseconds current time in nanoseconds + * \param received_message the message received by the subscription + * \param now_nanoseconds current time in nanoseconds */ virtual void OnMessageReceived( const CallbackMessageT & received_message, @@ -143,7 +143,7 @@ class SubscriberTopicStatistics /// Return a vector of all the currently collected data /** - * @return a vector of all the collected data + * \return a vector of all the collected data */ std::vector GetCurrentCollectorData() const { From 019c86d640249cba4ac07d52e14f65a29d0bb6a2 Mon Sep 17 00:00:00 2001 From: Devin Bonnie Date: Mon, 13 Apr 2020 14:35:29 -0700 Subject: [PATCH 06/25] Remove SetPublisherTimer method Signed-off-by: Devin Bonnie --- .../topic_statistics/subscriber_topic_statistics.hpp | 9 --------- 1 file changed, 9 deletions(-) diff --git a/rclcpp/include/rclcpp/topic_statistics/subscriber_topic_statistics.hpp b/rclcpp/include/rclcpp/topic_statistics/subscriber_topic_statistics.hpp index 9d1cbb36a3..e9c08a52cd 100644 --- a/rclcpp/include/rclcpp/topic_statistics/subscriber_topic_statistics.hpp +++ b/rclcpp/include/rclcpp/topic_statistics/subscriber_topic_statistics.hpp @@ -116,15 +116,6 @@ class SubscriberTopicStatistics TearDown(); } - /// Set the timer used to publish statistics messages. - /** - * \param measurement_timer the timer to fire the publisher, created by the node - */ - void SetPublisherTimer(const rclcpp::TimerBase::SharedPtr & publisher_timer) - { - publisher_timer_ = std::move(publisher_timer); - } - /// Handle a message received by the subscription to collect statistics. /** * \param received_message the message received by the subscription From 8b8a31cc10a257e6d7f79967492e4e53e96a0fbb Mon Sep 17 00:00:00 2001 From: Devin Bonnie Date: Mon, 13 Apr 2020 15:49:56 -0700 Subject: [PATCH 07/25] Change naming style to match rclcpp Signed-off-by: Devin Bonnie --- .../subscriber_topic_statistics.hpp | 24 +++++++++---------- .../test_subscriber_topic_statistics.cpp | 8 +++---- 2 files changed, 15 insertions(+), 17 deletions(-) diff --git a/rclcpp/include/rclcpp/topic_statistics/subscriber_topic_statistics.hpp b/rclcpp/include/rclcpp/topic_statistics/subscriber_topic_statistics.hpp index e9c08a52cd..cf1f8f3b7e 100644 --- a/rclcpp/include/rclcpp/topic_statistics/subscriber_topic_statistics.hpp +++ b/rclcpp/include/rclcpp/topic_statistics/subscriber_topic_statistics.hpp @@ -39,11 +39,9 @@ namespace { /// Return the current nanoseconds (count) since epoch. /** - * For now, use hard coded time instead of a node's clock (to support sim time and playback) - * due to node clock lifecycle issues. * \return the current nanoseconds (count) since epoch */ -int64_t GetCurrentNanosecondsSinceEpoch() +int64_t get_current_nanoseconds_since_epoch() { const auto now = std::chrono::system_clock::now(); return std::chrono::duration_cast(now.time_since_epoch()).count(); @@ -101,19 +99,19 @@ class SubscriberTopicStatistics auto callback = [this]() { - this->PublishMessage(); + this->publish_message(); }; publisher_timer_ = node.create_wall_timer(publishing_period, callback); node_name_ = node.get_name(); - BringUp(); + bring_up(); } virtual ~SubscriberTopicStatistics() { - TearDown(); + tear_down(); } /// Handle a message received by the subscription to collect statistics. @@ -121,7 +119,7 @@ class SubscriberTopicStatistics * \param received_message the message received by the subscription * \param now_nanoseconds current time in nanoseconds */ - virtual void OnMessageReceived( + virtual void handle_message( const CallbackMessageT & received_message, const rcl_time_point_value_t now_nanoseconds) const { @@ -136,7 +134,7 @@ class SubscriberTopicStatistics /** * \return a vector of all the collected data */ - std::vector GetCurrentCollectorData() const + std::vector get_current_collector_data() const { std::vector data; for (const auto & collector : subscriber_statistics_collectors_) { @@ -147,17 +145,17 @@ class SubscriberTopicStatistics private: /// Construct and start all collectors and set window_start_. - void BringUp() + void bring_up() { auto received_message_period = std::make_unique(); received_message_period->Start(); subscriber_statistics_collectors_.emplace_back(std::move(received_message_period)); - window_start_ = rclcpp::Time(GetCurrentNanosecondsSinceEpoch()); + window_start_ = rclcpp::Time(get_current_nanoseconds_since_epoch()); } /// Stop all collectors, clear measurements, stop publishing timer, and reset publisher. - void TearDown() + void tear_down() { for (auto & collector : subscriber_statistics_collectors_) { collector->Stop(); @@ -174,9 +172,9 @@ class SubscriberTopicStatistics } /// Publish a populated MetricsStatisticsMessage - virtual void PublishMessage() + virtual void publish_message() { - rclcpp::Time window_end{GetCurrentNanosecondsSinceEpoch()}; + rclcpp::Time window_end{get_current_nanoseconds_since_epoch()}; for (auto & collector : subscriber_statistics_collectors_) { const auto collected_stats = collector->GetStatisticsResults(); diff --git a/rclcpp/test/topic_statistics/test_subscriber_topic_statistics.cpp b/rclcpp/test/topic_statistics/test_subscriber_topic_statistics.cpp index 03f92a7935..782afef6f6 100644 --- a/rclcpp/test/topic_statistics/test_subscriber_topic_statistics.cpp +++ b/rclcpp/test/topic_statistics/test_subscriber_topic_statistics.cpp @@ -54,7 +54,7 @@ class EmptySubscriber : public rclcpp::Node : Node(name) { auto callback = [this](Empty::UniquePtr msg) { - this->ReceiveMessage(*msg); + this->receive_message(*msg); }; subscription_ = create_subscription>( @@ -64,7 +64,7 @@ class EmptySubscriber : public rclcpp::Node } private: - void ReceiveMessage(const Empty &) const + void receive_message(const Empty &) const { } @@ -93,14 +93,14 @@ class TestSubscriberTopicStatisticsFixture : public ::testing::Test std::shared_ptr empty_subscriber; }; -TEST_F(TestSubscriberTopicStatisticsFixture, TestManualConstruction) +TEST_F(TestSubscriberTopicStatisticsFixture, test_manual_construction) { // construct the instance auto sub_topic_stats = std::make_unique>( *empty_subscriber); // expect no data has been collected / no samples received - for (const auto & data : sub_topic_stats->GetCurrentCollectorData()) { + for (const auto & data : sub_topic_stats->get_current_collector_data()) { EXPECT_TRUE(std::isnan(data.average)); EXPECT_TRUE(std::isnan(data.min)); EXPECT_TRUE(std::isnan(data.max)); From ade51caddb06f0288492a9d39a568d57b02c5b8c Mon Sep 17 00:00:00 2001 From: Devin Bonnie Date: Tue, 14 Apr 2020 13:16:15 -0700 Subject: [PATCH 08/25] Address style issues Signed-off-by: Devin Bonnie --- .../subscriber_topic_statistics.hpp | 17 ++++++++--------- .../test_subscriber_topic_statistics.cpp | 5 ++--- 2 files changed, 10 insertions(+), 12 deletions(-) diff --git a/rclcpp/include/rclcpp/topic_statistics/subscriber_topic_statistics.hpp b/rclcpp/include/rclcpp/topic_statistics/subscriber_topic_statistics.hpp index cf1f8f3b7e..1b7021dfe1 100644 --- a/rclcpp/include/rclcpp/topic_statistics/subscriber_topic_statistics.hpp +++ b/rclcpp/include/rclcpp/topic_statistics/subscriber_topic_statistics.hpp @@ -20,21 +20,20 @@ #include #include +#include "libstatistics_collector/collector/generate_statistics_message.hpp" +#include "libstatistics_collector/moving_average_statistics/types.hpp" +#include "libstatistics_collector/topic_statistics_collector/constants.hpp" +#include "libstatistics_collector/topic_statistics_collector/received_message_period.hpp" + +#include "metrics_statistics_msgs/msg/metrics_message.hpp" -#include "rclcpp/create_publisher.hpp" #include "rcl/time.h" +#include "rclcpp/create_publisher.hpp" #include "rclcpp/node.hpp" -#include "rclcpp/time.hpp" #include "rclcpp/publisher.hpp" +#include "rclcpp/time.hpp" #include "rclcpp/timer.hpp" -#include "metrics_statistics_msgs/msg/metrics_message.hpp" - -#include "libstatistics_collector/collector/generate_statistics_message.hpp" -#include "libstatistics_collector/topic_statistics_collector/constants.hpp" -#include "libstatistics_collector/topic_statistics_collector/received_message_period.hpp" -#include "libstatistics_collector/moving_average_statistics/types.hpp" - namespace { /// Return the current nanoseconds (count) since epoch. diff --git a/rclcpp/test/topic_statistics/test_subscriber_topic_statistics.cpp b/rclcpp/test/topic_statistics/test_subscriber_topic_statistics.cpp index 782afef6f6..bfc5e8286b 100644 --- a/rclcpp/test/topic_statistics/test_subscriber_topic_statistics.cpp +++ b/rclcpp/test/topic_statistics/test_subscriber_topic_statistics.cpp @@ -22,15 +22,14 @@ #include "metrics_statistics_msgs/msg/metrics_message.hpp" #include "rclcpp/create_publisher.hpp" -#include "rclcpp/rclcpp.hpp" -#include "rclcpp/qos.hpp" #include "rclcpp/node.hpp" +#include "rclcpp/qos.hpp" +#include "rclcpp/rclcpp.hpp" #include "rclcpp/topic_statistics/subscriber_topic_statistics.hpp" #include "test_msgs/msg/empty.hpp" - namespace { constexpr const char kTestNodeName[]{"test_sub_stats_node"}; From 34a89ce747007e384cef97adf47508543abd5509 Mon Sep 17 00:00:00 2001 From: Devin Bonnie Date: Tue, 14 Apr 2020 15:13:03 -0700 Subject: [PATCH 09/25] Use rclcpp:Time Signed-off-by: Devin Bonnie --- .../rclcpp/topic_statistics/subscriber_topic_statistics.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/rclcpp/include/rclcpp/topic_statistics/subscriber_topic_statistics.hpp b/rclcpp/include/rclcpp/topic_statistics/subscriber_topic_statistics.hpp index 1b7021dfe1..83d3d477e1 100644 --- a/rclcpp/include/rclcpp/topic_statistics/subscriber_topic_statistics.hpp +++ b/rclcpp/include/rclcpp/topic_statistics/subscriber_topic_statistics.hpp @@ -120,12 +120,12 @@ class SubscriberTopicStatistics */ virtual void handle_message( const CallbackMessageT & received_message, - const rcl_time_point_value_t now_nanoseconds) const + const rclcpp::Time now_nanoseconds) const { (void) received_message; for (const auto & collector : subscriber_statistics_collectors_) { - collector->OnMessageReceived(received_message, now_nanoseconds); + collector->OnMessageReceived(received_message, now_nanoseconds.nanoseconds()); } } From c9a6eb7035a6d2085f61398e623d61122ff102d0 Mon Sep 17 00:00:00 2001 From: Devin Bonnie Date: Wed, 15 Apr 2020 00:11:59 -0700 Subject: [PATCH 10/25] Address review comments Signed-off-by: Devin Bonnie --- .../rclcpp/topic_statistics/subscriber_topic_statistics.hpp | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/rclcpp/include/rclcpp/topic_statistics/subscriber_topic_statistics.hpp b/rclcpp/include/rclcpp/topic_statistics/subscriber_topic_statistics.hpp index 83d3d477e1..c70eb2a63b 100644 --- a/rclcpp/include/rclcpp/topic_statistics/subscriber_topic_statistics.hpp +++ b/rclcpp/include/rclcpp/topic_statistics/subscriber_topic_statistics.hpp @@ -80,7 +80,7 @@ class SubscriberTopicStatistics /** * This object wraps utilities, defined in libstatistics_collector, to collect, * measure, and publish topic statistics data. - * + * * \param node the node creating the subscription, used to create the publisher and timer to * publish topic statistics. * \param publishing_topic the topic to publish statistics to @@ -122,8 +122,6 @@ class SubscriberTopicStatistics const CallbackMessageT & received_message, const rclcpp::Time now_nanoseconds) const { - (void) received_message; - for (const auto & collector : subscriber_statistics_collectors_) { collector->OnMessageReceived(received_message, now_nanoseconds.nanoseconds()); } From 6f7060a41bbd9fb3ff14424f57af68f4c6441565 Mon Sep 17 00:00:00 2001 From: Devin Bonnie Date: Wed, 15 Apr 2020 11:38:23 -0700 Subject: [PATCH 11/25] Remove unnecessary check for null publisher timer Move anonymous namespace function to private class method Signed-off-by: Devin Bonnie --- .../subscriber_topic_statistics.hpp | 29 ++++++++----------- 1 file changed, 12 insertions(+), 17 deletions(-) diff --git a/rclcpp/include/rclcpp/topic_statistics/subscriber_topic_statistics.hpp b/rclcpp/include/rclcpp/topic_statistics/subscriber_topic_statistics.hpp index c70eb2a63b..b860f8cd80 100644 --- a/rclcpp/include/rclcpp/topic_statistics/subscriber_topic_statistics.hpp +++ b/rclcpp/include/rclcpp/topic_statistics/subscriber_topic_statistics.hpp @@ -34,19 +34,6 @@ #include "rclcpp/time.hpp" #include "rclcpp/timer.hpp" -namespace -{ -/// Return the current nanoseconds (count) since epoch. -/** - * \return the current nanoseconds (count) since epoch - */ -int64_t get_current_nanoseconds_since_epoch() -{ - const auto now = std::chrono::system_clock::now(); - return std::chrono::duration_cast(now.time_since_epoch()).count(); -} -} // namespace - namespace rclcpp { namespace topic_statistics @@ -160,10 +147,8 @@ class SubscriberTopicStatistics subscriber_statistics_collectors_.clear(); - if (publisher_timer_) { - publisher_timer_->cancel(); - publisher_timer_.reset(); - } + publisher_timer_->cancel(); + publisher_timer_.reset(); publisher_.reset(); } @@ -188,6 +173,16 @@ class SubscriberTopicStatistics window_start_ = window_end; } + /// Return the current nanoseconds (count) since epoch. + /** + * \return the current nanoseconds (count) since epoch + */ + int64_t get_current_nanoseconds_since_epoch() + { + const auto now = std::chrono::system_clock::now(); + return std::chrono::duration_cast(now.time_since_epoch()).count(); + } + /// Collection of statistics collectors std::vector> subscriber_statistics_collectors_{}; /// Node name used to generate topic statistics messages to be published From 9c19234b28debbeb06471697268a86805a41a084 Mon Sep 17 00:00:00 2001 From: Devin Bonnie Date: Fri, 17 Apr 2020 14:24:57 -0700 Subject: [PATCH 12/25] Update message dependency Signed-off-by: Devin Bonnie --- rclcpp/CMakeLists.txt | 1 - .../topic_statistics/subscriber_topic_statistics.hpp | 8 ++++---- .../topic_statistics/test_subscriber_topic_statistics.cpp | 5 ++--- 3 files changed, 6 insertions(+), 8 deletions(-) diff --git a/rclcpp/CMakeLists.txt b/rclcpp/CMakeLists.txt index 9d5ff91f94..3918120e5a 100644 --- a/rclcpp/CMakeLists.txt +++ b/rclcpp/CMakeLists.txt @@ -5,7 +5,6 @@ project(rclcpp) find_package(ament_cmake_ros REQUIRED) find_package(builtin_interfaces REQUIRED) find_package(libstatistics_collector REQUIRED) -find_package(metrics_statistics_msgs REQUIRED) find_package(rcl REQUIRED) find_package(rcl_interfaces REQUIRED) find_package(rcl_yaml_param_parser REQUIRED) diff --git a/rclcpp/include/rclcpp/topic_statistics/subscriber_topic_statistics.hpp b/rclcpp/include/rclcpp/topic_statistics/subscriber_topic_statistics.hpp index b860f8cd80..12e071dc6f 100644 --- a/rclcpp/include/rclcpp/topic_statistics/subscriber_topic_statistics.hpp +++ b/rclcpp/include/rclcpp/topic_statistics/subscriber_topic_statistics.hpp @@ -25,7 +25,7 @@ #include "libstatistics_collector/topic_statistics_collector/constants.hpp" #include "libstatistics_collector/topic_statistics_collector/received_message_period.hpp" -#include "metrics_statistics_msgs/msg/metrics_message.hpp" +#include "statistics_msgs/msg/metrics_message.hpp" #include "rcl/time.h" #include "rclcpp/create_publisher.hpp" @@ -43,7 +43,7 @@ constexpr const char kDefaultPublishTopicName[]{"topic_statistics"}; constexpr const std::chrono::milliseconds kDefaultPublishingPeriod{std::chrono::minutes(1)}; using libstatistics_collector::collector::GenerateStatisticMessage; -using metrics_statistics_msgs::msg::MetricsMessage; +using statistics_msgs::msg::MetricsMessage; using libstatistics_collector::moving_average_statistics::StatisticData; /** @@ -79,7 +79,7 @@ class SubscriberTopicStatistics const std::chrono::milliseconds & publishing_period = kDefaultPublishingPeriod) { publisher_ = - node.create_publisher( + node.create_publisher( publishing_topic, 10); @@ -188,7 +188,7 @@ class SubscriberTopicStatistics /// Node name used to generate topic statistics messages to be published std::string node_name_; /// Publisher, created by the node, used to publish topic statistics messages - rclcpp::Publisher::SharedPtr publisher_; + rclcpp::Publisher::SharedPtr publisher_; /// Timer which fires the publisher rclcpp::TimerBase::SharedPtr publisher_timer_; /// The start of the collection window, used in the published topic statistics message diff --git a/rclcpp/test/topic_statistics/test_subscriber_topic_statistics.cpp b/rclcpp/test/topic_statistics/test_subscriber_topic_statistics.cpp index bfc5e8286b..ab5f008ac3 100644 --- a/rclcpp/test/topic_statistics/test_subscriber_topic_statistics.cpp +++ b/rclcpp/test/topic_statistics/test_subscriber_topic_statistics.cpp @@ -19,8 +19,6 @@ #include "libstatistics_collector/moving_average_statistics/types.hpp" -#include "metrics_statistics_msgs/msg/metrics_message.hpp" - #include "rclcpp/create_publisher.hpp" #include "rclcpp/node.hpp" #include "rclcpp/qos.hpp" @@ -28,6 +26,7 @@ #include "rclcpp/topic_statistics/subscriber_topic_statistics.hpp" +#include "statistics_msgs/msg/metrics_message.hpp" #include "test_msgs/msg/empty.hpp" namespace @@ -39,7 +38,7 @@ constexpr const uint64_t kNoSamples{0}; } // namespace using test_msgs::msg::Empty; -using metrics_statistics_msgs::msg::MetricsMessage; +using statistics_msgs::msg::MetricsMessage; using rclcpp::topic_statistics::SubscriberTopicStatistics; using libstatistics_collector::moving_average_statistics::StatisticData; From 95547848035136dfbd596b8df11925748c9c8214 Mon Sep 17 00:00:00 2001 From: Devin Bonnie Date: Fri, 17 Apr 2020 19:48:54 -0700 Subject: [PATCH 13/25] Initial integration of Subscriber Topic Statistics Signed-off-by: Devin Bonnie --- rclcpp/include/rclcpp/create_subscription.hpp | 8 +- rclcpp/include/rclcpp/node_impl.hpp | 34 ++++++ rclcpp/include/rclcpp/subscription.hpp | 20 +++- .../include/rclcpp/subscription_factory.hpp | 11 +- .../subscriber_topic_statistics.hpp | 110 +++++++++--------- .../test_subscriber_topic_statistics.cpp | 13 ++- 6 files changed, 134 insertions(+), 62 deletions(-) diff --git a/rclcpp/include/rclcpp/create_subscription.hpp b/rclcpp/include/rclcpp/create_subscription.hpp index 9248254047..89330cdc3f 100644 --- a/rclcpp/include/rclcpp/create_subscription.hpp +++ b/rclcpp/include/rclcpp/create_subscription.hpp @@ -24,6 +24,7 @@ #include "rclcpp/subscription_factory.hpp" #include "rclcpp/subscription_options.hpp" #include "rclcpp/qos.hpp" +#include "rclcpp/topic_statistics/subscriber_topic_statistics.hpp" #include "rmw/qos_profiles.h" namespace rclcpp @@ -58,7 +59,9 @@ create_subscription( ), typename MessageMemoryStrategyT::SharedPtr msg_mem_strat = ( MessageMemoryStrategyT::create_default() - ) + ), + std::shared_ptr> + subscriber_topic_stats = nullptr ) { using rclcpp::node_interfaces::get_node_topics_interface; @@ -67,7 +70,8 @@ create_subscription( auto factory = rclcpp::create_subscription_factory( std::forward(callback), options, - msg_mem_strat + msg_mem_strat, + subscriber_topic_stats ); auto sub = node_topics->create_subscription(topic_name, factory, qos); diff --git a/rclcpp/include/rclcpp/node_impl.hpp b/rclcpp/include/rclcpp/node_impl.hpp index e29edeb22e..f41073f477 100644 --- a/rclcpp/include/rclcpp/node_impl.hpp +++ b/rclcpp/include/rclcpp/node_impl.hpp @@ -43,6 +43,11 @@ #include "rclcpp/qos.hpp" #include "rclcpp/type_support_decl.hpp" #include "rclcpp/visibility_control.hpp" +#include "rclcpp/timer.hpp" + +#include "rclcpp/create_publisher.hpp" + +#include "statistics_msgs/msg/metrics_message.hpp" #ifndef RCLCPP__NODE_HPP_ #include "node.hpp" @@ -92,6 +97,35 @@ Node::create_subscription( const SubscriptionOptionsWithAllocator & options, typename MessageMemoryStrategyT::SharedPtr msg_mem_strat) { + //todo only set if the option is set + if (true) + { + std::shared_ptr> publisher = + this->create_publisher("topic_statistics", rclcpp::QoS(10)); + + // todo fix name + auto sub_topic_stats = std::make_shared< + rclcpp::topic_statistics::SubscriberTopicStatistics + >(this->get_name(), publisher); + + //todo (dabonnie): fix hardcoded duration + auto timer = this->create_wall_timer(std::chrono::seconds{15}, [sub_topic_stats] () { + sub_topic_stats->publish_message(); + }); + + sub_topic_stats->set_publisher_timer(timer); + + return rclcpp::create_subscription( + *this, + extend_name_with_sub_namespace(topic_name, this->get_sub_namespace()), + qos, + std::forward(callback), + options, + msg_mem_strat, + sub_topic_stats); + } + //TODO (dabonnie): fix QoS, configurable or hardcoded? + return rclcpp::create_subscription( *this, extend_name_with_sub_namespace(topic_name, this->get_sub_namespace()), diff --git a/rclcpp/include/rclcpp/subscription.hpp b/rclcpp/include/rclcpp/subscription.hpp index 2b5172dc1b..394aa97e64 100644 --- a/rclcpp/include/rclcpp/subscription.hpp +++ b/rclcpp/include/rclcpp/subscription.hpp @@ -18,6 +18,7 @@ #include #include +#include #include #include #include @@ -25,7 +26,6 @@ #include #include - #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/subscriber_topic_statistics.hpp" #include "tracetools/tracetools.h" namespace rclcpp @@ -75,6 +76,8 @@ class Subscription : public SubscriptionBase using MessageDeleter = allocator::Deleter; using ConstMessageSharedPtr = std::shared_ptr; using MessageUniquePtr = std::unique_ptr; + using SubscriberTopicStatisticsSharedPtr = + std::shared_ptr>; RCLCPP_SMART_PTR_DEFINITIONS(Subscription) @@ -98,7 +101,8 @@ class Subscription : public SubscriptionBase const rclcpp::QoS & qos, AnySubscriptionCallback callback, const rclcpp::SubscriptionOptionsWithAllocator & options, - typename MessageMemoryStrategyT::SharedPtr message_memory_strategy) + typename MessageMemoryStrategyT::SharedPtr message_memory_strategy, + SubscriberTopicStatisticsSharedPtr subscriber_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 (subscriber_topic_statistics != nullptr) { + this->subscriber_topic_statistics_ = std::move(subscriber_topic_statistics); + } + TRACEPOINT( rclcpp_subscription_init, (const void *)get_subscription_handle().get(), @@ -260,6 +268,12 @@ class Subscription : public SubscriptionBase } auto typed_message = std::static_pointer_cast(message); any_callback_.dispatch(typed_message, message_info); + + if (subscriber_topic_statistics_) { + const auto nanos = std::chrono::time_point_cast(std::chrono::steady_clock::now()); + const auto time = rclcpp::Time(nanos.time_since_epoch().count()); + subscriber_topic_statistics_->handle_message(*typed_message, time); + } } void @@ -307,6 +321,8 @@ class Subscription : public SubscriptionBase const rclcpp::SubscriptionOptionsWithAllocator options_; typename message_memory_strategy::MessageMemoryStrategy::SharedPtr message_memory_strategy_; + /// Component which computes and publishes topic statistics for this subscriber + SubscriberTopicStatisticsSharedPtr subscriber_topic_statistics_{nullptr}; }; } // namespace rclcpp diff --git a/rclcpp/include/rclcpp/subscription_factory.hpp b/rclcpp/include/rclcpp/subscription_factory.hpp index a0f265c803..3b3be2a782 100644 --- 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/subscriber_topic_statistics.hpp" namespace rclcpp { @@ -78,7 +79,10 @@ SubscriptionFactory create_subscription_factory( CallbackT && callback, const rclcpp::SubscriptionOptionsWithAllocator & options, - typename MessageMemoryStrategyT::SharedPtr msg_mem_strat) + typename MessageMemoryStrategyT::SharedPtr msg_mem_strat, + std::shared_ptr> + subscriber_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, subscriber_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, + subscriber_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. diff --git a/rclcpp/include/rclcpp/topic_statistics/subscriber_topic_statistics.hpp b/rclcpp/include/rclcpp/topic_statistics/subscriber_topic_statistics.hpp index 12e071dc6f..e6d5487201 100644 --- a/rclcpp/include/rclcpp/topic_statistics/subscriber_topic_statistics.hpp +++ b/rclcpp/include/rclcpp/topic_statistics/subscriber_topic_statistics.hpp @@ -20,6 +20,11 @@ #include #include +#include "rcl/time.h" +#include "rclcpp/time.hpp" +#include "rclcpp/publisher.hpp" +#include "rclcpp/timer.hpp" + #include "libstatistics_collector/collector/generate_statistics_message.hpp" #include "libstatistics_collector/moving_average_statistics/types.hpp" #include "libstatistics_collector/topic_statistics_collector/constants.hpp" @@ -27,13 +32,6 @@ #include "statistics_msgs/msg/metrics_message.hpp" -#include "rcl/time.h" -#include "rclcpp/create_publisher.hpp" -#include "rclcpp/node.hpp" -#include "rclcpp/publisher.hpp" -#include "rclcpp/time.hpp" -#include "rclcpp/timer.hpp" - namespace rclcpp { namespace topic_statistics @@ -63,36 +61,31 @@ class SubscriberTopicStatistics CallbackMessageT>; public: - /// Construct a SubcriberTopicStatistics object. + /// Construct a SubscriberTopicStatistics object. /** - * This object wraps utilities, defined in libstatistics_collector, to collect, - * measure, and publish topic statistics data. - * - * \param node the node creating the subscription, used to create the publisher and timer to - * publish topic statistics. - * \param publishing_topic the topic to publish statistics to - * \param publishing_period the period at which topic statistics messages are published - */ + * This object wraps utilities, defined in libstatistics_collector, to collect, + * measure, and publish topic statistics data. + * + * @param node_name the name of the node, which created this instance, in order to denote + * topic source + * @param publisher instance constructed by the node in order to publish statistics data + */ SubscriberTopicStatistics( - rclcpp::Node & node, - const std::string & publishing_topic = kDefaultPublishTopicName, - const std::chrono::milliseconds & publishing_period = kDefaultPublishingPeriod) + const std::string & node_name, + const rclcpp::Publisher::SharedPtr & publisher) + : node_name_(node_name), + publisher_(std::move(publisher)) { - publisher_ = - node.create_publisher( - publishing_topic, - 10); + // TODO(dbbonnie): ros-tooling/aws-roadmap/issues/226, received message age - auto callback = [this]() - { - this->publish_message(); - }; - - publisher_timer_ = node.create_wall_timer(publishing_period, callback); + bring_up(); + } - node_name_ = node.get_name(); + SubscriberTopicStatistics( + const std::string & publishing_topic = kDefaultPublishTopicName, + const std::chrono::milliseconds & publishing_period = kDefaultPublishingPeriod) + { - bring_up(); } virtual ~SubscriberTopicStatistics() @@ -127,6 +120,35 @@ class SubscriberTopicStatistics return data; } + /// Set the timer used to publish statistics messages. + /** + * @param measurement_timer the timer to fire the publisher, created by the node + */ + void set_publisher_timer(const rclcpp::TimerBase::SharedPtr & publisher_timer) + { + publisher_timer_ = publisher_timer; + } + + /// Publish a populated MetricsStatisticsMessage + virtual void publish_message() + { + rclcpp::Time window_end{get_current_nanoseconds_since_epoch()}; + + for (auto & collector : subscriber_statistics_collectors_) { + const auto collected_stats = collector->GetStatisticsResults(); + + auto message = libstatistics_collector::collector::GenerateStatisticMessage( + node_name_, + collector->GetMetricName(), + collector->GetMetricUnit(), + window_start_, + window_end, + collected_stats); + publisher_->publish(message); + } + window_start_ = window_end; + } + private: /// Construct and start all collectors and set window_start_. void bring_up() @@ -147,32 +169,14 @@ class SubscriberTopicStatistics subscriber_statistics_collectors_.clear(); - publisher_timer_->cancel(); - publisher_timer_.reset(); + if (publisher_timer_) { + publisher_timer_->cancel(); + publisher_timer_.reset(); + } publisher_.reset(); } - /// Publish a populated MetricsStatisticsMessage - virtual void publish_message() - { - rclcpp::Time window_end{get_current_nanoseconds_since_epoch()}; - - for (auto & collector : subscriber_statistics_collectors_) { - const auto collected_stats = collector->GetStatisticsResults(); - - auto message = libstatistics_collector::collector::GenerateStatisticMessage( - node_name_, - collector->GetMetricName(), - collector->GetMetricUnit(), - window_start_, - window_end, - collected_stats); - publisher_->publish(message); - } - window_start_ = window_end; - } - /// Return the current nanoseconds (count) since epoch. /** * \return the current nanoseconds (count) since epoch diff --git a/rclcpp/test/topic_statistics/test_subscriber_topic_statistics.cpp b/rclcpp/test/topic_statistics/test_subscriber_topic_statistics.cpp index ab5f008ac3..d93fb30a37 100644 --- a/rclcpp/test/topic_statistics/test_subscriber_topic_statistics.cpp +++ b/rclcpp/test/topic_statistics/test_subscriber_topic_statistics.cpp @@ -93,9 +93,18 @@ class TestSubscriberTopicStatisticsFixture : public ::testing::Test TEST_F(TestSubscriberTopicStatisticsFixture, test_manual_construction) { + // manually create publisher tied to the node + auto topic_stats_publisher = + empty_subscriber->create_publisher( + kTestTopicStatisticsTopic, + 10); + // construct the instance auto sub_topic_stats = std::make_unique>( - *empty_subscriber); + empty_subscriber->get_name(), + topic_stats_publisher); + + using libstatistics_collector::moving_average_statistics::StatisticData; // expect no data has been collected / no samples received for (const auto & data : sub_topic_stats->get_current_collector_data()) { @@ -105,4 +114,4 @@ TEST_F(TestSubscriberTopicStatisticsFixture, test_manual_construction) EXPECT_TRUE(std::isnan(data.standard_deviation)); EXPECT_EQ(kNoSamples, data.sample_count); } -} +} \ No newline at end of file From c4519ebf28a7155f344f2b9a109eba78fba7d1e3 Mon Sep 17 00:00:00 2001 From: Devin Bonnie Date: Sat, 18 Apr 2020 12:00:20 -0700 Subject: [PATCH 14/25] Fix nanoseconds used for Topic Stats Signed-off-by: Devin Bonnie --- rclcpp/include/rclcpp/subscription.hpp | 2 +- .../topic_statistics/subscriber_topic_statistics.hpp | 8 -------- 2 files changed, 1 insertion(+), 9 deletions(-) diff --git a/rclcpp/include/rclcpp/subscription.hpp b/rclcpp/include/rclcpp/subscription.hpp index 394aa97e64..566de6f2bb 100644 --- a/rclcpp/include/rclcpp/subscription.hpp +++ b/rclcpp/include/rclcpp/subscription.hpp @@ -271,7 +271,7 @@ class Subscription : public SubscriptionBase if (subscriber_topic_statistics_) { const auto nanos = std::chrono::time_point_cast(std::chrono::steady_clock::now()); - const auto time = rclcpp::Time(nanos.time_since_epoch().count()); + const auto time = rclcpp::Time(nanos.count()); subscriber_topic_statistics_->handle_message(*typed_message, time); } } diff --git a/rclcpp/include/rclcpp/topic_statistics/subscriber_topic_statistics.hpp b/rclcpp/include/rclcpp/topic_statistics/subscriber_topic_statistics.hpp index e6d5487201..85cb345938 100644 --- a/rclcpp/include/rclcpp/topic_statistics/subscriber_topic_statistics.hpp +++ b/rclcpp/include/rclcpp/topic_statistics/subscriber_topic_statistics.hpp @@ -77,17 +77,9 @@ class SubscriberTopicStatistics publisher_(std::move(publisher)) { // TODO(dbbonnie): ros-tooling/aws-roadmap/issues/226, received message age - bring_up(); } - SubscriberTopicStatistics( - const std::string & publishing_topic = kDefaultPublishTopicName, - const std::chrono::milliseconds & publishing_period = kDefaultPublishingPeriod) - { - - } - virtual ~SubscriberTopicStatistics() { tear_down(); From 598d0458cfdc28885cfbe6675be75b61652b5370 Mon Sep 17 00:00:00 2001 From: Devin Bonnie Date: Sat, 18 Apr 2020 14:22:29 -0700 Subject: [PATCH 15/25] Add simple publishing test Minor fixes Signed-off-by: Devin Bonnie --- rclcpp/include/rclcpp/create_subscription.hpp | 2 +- rclcpp/include/rclcpp/node_impl.hpp | 27 ++-- rclcpp/include/rclcpp/subscription.hpp | 5 +- .../include/rclcpp/subscription_factory.hpp | 2 +- .../subscriber_topic_statistics.hpp | 8 +- .../test_subscriber_topic_statistics.cpp | 118 ++++++++++++++++-- 6 files changed, 132 insertions(+), 30 deletions(-) diff --git a/rclcpp/include/rclcpp/create_subscription.hpp b/rclcpp/include/rclcpp/create_subscription.hpp index 89330cdc3f..b41d7f5895 100644 --- a/rclcpp/include/rclcpp/create_subscription.hpp +++ b/rclcpp/include/rclcpp/create_subscription.hpp @@ -61,7 +61,7 @@ create_subscription( MessageMemoryStrategyT::create_default() ), std::shared_ptr> - subscriber_topic_stats = nullptr + subscriber_topic_stats = nullptr ) { using rclcpp::node_interfaces::get_node_topics_interface; diff --git a/rclcpp/include/rclcpp/node_impl.hpp b/rclcpp/include/rclcpp/node_impl.hpp index f41073f477..b7223034a9 100644 --- a/rclcpp/include/rclcpp/node_impl.hpp +++ b/rclcpp/include/rclcpp/node_impl.hpp @@ -45,8 +45,6 @@ #include "rclcpp/visibility_control.hpp" #include "rclcpp/timer.hpp" -#include "rclcpp/create_publisher.hpp" - #include "statistics_msgs/msg/metrics_message.hpp" #ifndef RCLCPP__NODE_HPP_ @@ -97,21 +95,24 @@ Node::create_subscription( const SubscriptionOptionsWithAllocator & options, typename MessageMemoryStrategyT::SharedPtr msg_mem_strat) { - //todo only set if the option is set - if (true) - { + // TODO(dabonnie): only set if the option is set, via https://github.com/ros2/rclcpp/pull/1057 + if (true) { + // TODO(dabonnie): fix QoS, configurable or hardcoded? std::shared_ptr> publisher = - this->create_publisher("topic_statistics", rclcpp::QoS(10)); + this->create_publisher( + "topic_statistics", + rclcpp::QoS(10)); - // todo fix name + // TODO(dabonnie): fix name via https://github.com/ros2/rclcpp/pull/1057 auto sub_topic_stats = std::make_shared< rclcpp::topic_statistics::SubscriberTopicStatistics - >(this->get_name(), publisher); + >(this->get_name(), publisher); - //todo (dabonnie): fix hardcoded duration - auto timer = this->create_wall_timer(std::chrono::seconds{15}, [sub_topic_stats] () { - sub_topic_stats->publish_message(); - }); + // TODO(dabonnie): fix hardcoded duration via https://github.com/ros2/rclcpp/pull/1057 + auto timer = this->create_wall_timer( + std::chrono::seconds{10}, [sub_topic_stats]() { + sub_topic_stats->publish_message(); + }); sub_topic_stats->set_publisher_timer(timer); @@ -124,8 +125,8 @@ Node::create_subscription( msg_mem_strat, sub_topic_stats); } - //TODO (dabonnie): fix QoS, configurable or hardcoded? + // TODO(dabonnie): fixme return rclcpp::create_subscription( *this, extend_name_with_sub_namespace(topic_name, this->get_sub_namespace()), diff --git a/rclcpp/include/rclcpp/subscription.hpp b/rclcpp/include/rclcpp/subscription.hpp index 566de6f2bb..3a2de416e4 100644 --- a/rclcpp/include/rclcpp/subscription.hpp +++ b/rclcpp/include/rclcpp/subscription.hpp @@ -270,8 +270,9 @@ class Subscription : public SubscriptionBase any_callback_.dispatch(typed_message, message_info); if (subscriber_topic_statistics_) { - const auto nanos = std::chrono::time_point_cast(std::chrono::steady_clock::now()); - const auto time = rclcpp::Time(nanos.count()); + const auto nanos = std::chrono::time_point_cast( + std::chrono::steady_clock::now()); + const auto time = rclcpp::Time(nanos.time_since_epoch().count()); subscriber_topic_statistics_->handle_message(*typed_message, time); } } diff --git a/rclcpp/include/rclcpp/subscription_factory.hpp b/rclcpp/include/rclcpp/subscription_factory.hpp index 3b3be2a782..0430ed071d 100644 --- a/rclcpp/include/rclcpp/subscription_factory.hpp +++ b/rclcpp/include/rclcpp/subscription_factory.hpp @@ -81,7 +81,7 @@ create_subscription_factory( const rclcpp::SubscriptionOptionsWithAllocator & options, typename MessageMemoryStrategyT::SharedPtr msg_mem_strat, std::shared_ptr> - subscriber_topic_stats = nullptr + subscriber_topic_stats = nullptr ) { auto allocator = options.get_allocator(); diff --git a/rclcpp/include/rclcpp/topic_statistics/subscriber_topic_statistics.hpp b/rclcpp/include/rclcpp/topic_statistics/subscriber_topic_statistics.hpp index 85cb345938..4eeae0137e 100644 --- a/rclcpp/include/rclcpp/topic_statistics/subscriber_topic_statistics.hpp +++ b/rclcpp/include/rclcpp/topic_statistics/subscriber_topic_statistics.hpp @@ -72,11 +72,11 @@ class SubscriberTopicStatistics */ SubscriberTopicStatistics( const std::string & node_name, - const rclcpp::Publisher::SharedPtr & publisher) - : node_name_(node_name), - publisher_(std::move(publisher)) + rclcpp::Publisher::SharedPtr & publisher) + : node_name_(node_name), + publisher_(std::move(publisher)) { - // TODO(dbbonnie): ros-tooling/aws-roadmap/issues/226, received message age + // TODO(dabonnie): ros-tooling/aws-roadmap/issues/226, received message age bring_up(); } diff --git a/rclcpp/test/topic_statistics/test_subscriber_topic_statistics.cpp b/rclcpp/test/topic_statistics/test_subscriber_topic_statistics.cpp index d93fb30a37..56b9a6c389 100644 --- a/rclcpp/test/topic_statistics/test_subscriber_topic_statistics.cpp +++ b/rclcpp/test/topic_statistics/test_subscriber_topic_statistics.cpp @@ -14,6 +14,9 @@ #include +#include +#include +#include #include #include @@ -26,21 +29,65 @@ #include "rclcpp/topic_statistics/subscriber_topic_statistics.hpp" +#include "test_topic_stats_utils.hpp" + #include "statistics_msgs/msg/metrics_message.hpp" +#include "statistics_msgs/msg/statistic_data_point.hpp" +#include "statistics_msgs/msg/statistic_data_type.hpp" + #include "test_msgs/msg/empty.hpp" namespace { -constexpr const char kTestNodeName[]{"test_sub_stats_node"}; +constexpr const char kTestPubNodeName[]{"test_pub_stats_node"}; +constexpr const char kTestSubNodeName[]{"test_sub_stats_node"}; constexpr const char kTestSubStatsTopic[]{"/test_sub_stats_topic"}; constexpr const char kTestTopicStatisticsTopic[]{"/test_topic_statistics_topic"}; constexpr const uint64_t kNoSamples{0}; +// TODO(dabonnie) fix when period is configurable +constexpr const std::chrono::seconds kTestDuration{20}; } // namespace using test_msgs::msg::Empty; -using statistics_msgs::msg::MetricsMessage; using rclcpp::topic_statistics::SubscriberTopicStatistics; -using libstatistics_collector::moving_average_statistics::StatisticData; +using statistics_msgs::msg::MetricsMessage; +using statistics_msgs::msg::StatisticDataPoint; +using statistics_msgs::msg::StatisticDataType; +/** + * Empty publisher node: used to publish empty messages + */ +class EmptyPublisher : public rclcpp::Node +{ +public: + EmptyPublisher( + const std::string & name, const std::string & topic, + const std::chrono::milliseconds & publish_period = std::chrono::milliseconds{100}) + : Node(name) + { + publisher_ = create_publisher(topic, 10); + publish_timer_ = this->create_wall_timer( + publish_period, [this]() { + this->publish_message(); + }); + } + + int get_number_published() + { + return number_published_.load(); + } + +private: + void publish_message() + { + ++number_published_; + auto msg = Empty{}; + publisher_->publish(msg); + } + + rclcpp::Publisher::SharedPtr publisher_; + std::atomic number_published_{0}; + rclcpp::TimerBase::SharedPtr publish_timer_; +}; /** * Empty subscriber node: used to create subscriber topic statistics requirements @@ -65,7 +112,6 @@ class EmptySubscriber : public rclcpp::Node void receive_message(const Empty &) const { } - rclcpp::Subscription::SharedPtr subscription_; }; @@ -79,7 +125,7 @@ class TestSubscriberTopicStatisticsFixture : public ::testing::Test { rclcpp::init(0 /* argc */, nullptr /* argv */); empty_subscriber = std::make_shared( - kTestNodeName, + kTestSubNodeName, kTestSubStatsTopic); } @@ -96,10 +142,10 @@ TEST_F(TestSubscriberTopicStatisticsFixture, test_manual_construction) // manually create publisher tied to the node auto topic_stats_publisher = empty_subscriber->create_publisher( - kTestTopicStatisticsTopic, - 10); + kTestTopicStatisticsTopic, + 10); - // construct the instance + // construct a separate instance auto sub_topic_stats = std::make_unique>( empty_subscriber->get_name(), topic_stats_publisher); @@ -114,4 +160,58 @@ TEST_F(TestSubscriberTopicStatisticsFixture, test_manual_construction) EXPECT_TRUE(std::isnan(data.standard_deviation)); EXPECT_EQ(kNoSamples, data.sample_count); } -} \ No newline at end of file +} + +TEST_F(TestSubscriberTopicStatisticsFixture, test_receive_single_empty_stats_message) +{ + // create an empty publisher + auto empty_publisher = std::make_shared( + kTestPubNodeName, + kTestSubStatsTopic); + // empty_subscriber has a topic statistics instance as part of its subscription + // this will listen to and generate statistics for the empty message + + // create a listener for topic statistics messages + auto statistics_listener = std::make_shared( + "test_receive_single_empty_stats_message_listener", + "/topic_statistics"); + + rclcpp::executors::SingleThreadedExecutor ex; + ex.add_node(empty_publisher); + ex.add_node(statistics_listener); + ex.add_node(empty_subscriber); + + // spin and get future + ex.spin_until_future_complete( + statistics_listener->GetFuture(), + kTestDuration); + + // compare message counts, sample count should be the same as published and received count + EXPECT_EQ(1, statistics_listener->GetNumberOfMessagesReceived()); + + // check the received message and the data types + const auto received_message = statistics_listener->GetLastReceivedMessage(); + for (const auto & stats_point : received_message.statistics) { + const auto type = stats_point.data_type; + switch (type) { + case StatisticDataType::STATISTICS_DATA_TYPE_SAMPLE_COUNT: + EXPECT_LT(0, stats_point.data) << "unexpected sample count"; + break; + case StatisticDataType::STATISTICS_DATA_TYPE_AVERAGE: + EXPECT_LT(0, stats_point.data) << "unexpected avg"; + break; + case StatisticDataType::STATISTICS_DATA_TYPE_MINIMUM: + EXPECT_LT(0, stats_point.data) << "unexpected min"; + break; + case StatisticDataType::STATISTICS_DATA_TYPE_MAXIMUM: + EXPECT_LT(0, stats_point.data) << "unexpected max"; + break; + case StatisticDataType::STATISTICS_DATA_TYPE_STDDEV: + EXPECT_LT(0, stats_point.data) << "unexpected stddev"; + break; + default: + FAIL() << "received unknown statistics type: " << std::dec << + static_cast(type); + } + } +} From ebc753e2c0a8e92c2144b67303ba3fd6c17052f0 Mon Sep 17 00:00:00 2001 From: Devin Bonnie Date: Sat, 18 Apr 2020 15:10:56 -0700 Subject: [PATCH 16/25] Add test utils header Signed-off-by: Devin Bonnie --- rclcpp/include/rclcpp/node_impl.hpp | 3 +- .../test_topic_stats_utils.hpp | 151 ++++++++++++++++++ 2 files changed, 153 insertions(+), 1 deletion(-) create mode 100644 rclcpp/test/topic_statistics/test_topic_stats_utils.hpp diff --git a/rclcpp/include/rclcpp/node_impl.hpp b/rclcpp/include/rclcpp/node_impl.hpp index b7223034a9..631e98e288 100644 --- a/rclcpp/include/rclcpp/node_impl.hpp +++ b/rclcpp/include/rclcpp/node_impl.hpp @@ -46,6 +46,7 @@ #include "rclcpp/timer.hpp" #include "statistics_msgs/msg/metrics_message.hpp" +#include #ifndef RCLCPP__NODE_HPP_ #include "node.hpp" @@ -100,7 +101,7 @@ Node::create_subscription( // TODO(dabonnie): fix QoS, configurable or hardcoded? std::shared_ptr> publisher = this->create_publisher( - "topic_statistics", + "topic_statistics" + std::to_string(rand()), // for now make unique rclcpp::QoS(10)); // TODO(dabonnie): fix name via https://github.com/ros2/rclcpp/pull/1057 diff --git a/rclcpp/test/topic_statistics/test_topic_stats_utils.hpp b/rclcpp/test/topic_statistics/test_topic_stats_utils.hpp new file mode 100644 index 0000000000..704556055b --- /dev/null +++ b/rclcpp/test/topic_statistics/test_topic_stats_utils.hpp @@ -0,0 +1,151 @@ +// Copyright 2020 Amazon.com, Inc. or its affiliates. All Rights Reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include +#include +#include + +#include "statistics_msgs/msg/metrics_message.hpp" + +#ifndef TOPIC_STATISTICS__TEST_TOPIC_STATS_UTILS_HPP_ +#define TOPIC_STATISTICS__TEST_TOPIC_STATS_UTILS_HPP_ + +namespace rclcpp +{ +namespace topic_statistics +{ + +using statistics_msgs::msg::MetricsMessage; + +/** +* Provide an interface to wait for a promise to be satisfied via its future. +*/ +class PromiseSetter +{ +public: + /** + * Reassign the promise member and return it's future. Acquires a mutex in order + * to mutate member variables. + * + * @return the promise member's future, called upon PeriodicMeasurement + */ + std::shared_future GetFuture() + { + std::unique_lock ulock{mutex_}; + use_future_ = true; + promise_ = std::promise(); + return promise_.get_future(); + } + +protected: + /** + * Set the promise to true, which signals the corresponding future. Acquires a mutex and sets + * the promise to true iff GetFuture was invoked before this. + */ + void SetPromise() + { + std::unique_lock ulock{mutex_}; + if (use_future_) { + // only set if GetFuture was called + promise_.set_value(true); + use_future_ = false; // the promise needs to be reassigned to set again + } + } + +private: + mutable std::mutex mutex_; + std::promise promise_; + bool use_future_{false}; +}; + +/** + * Node which listens for published MetricsMessages. This uses the PromiseSetter API + * in order to signal, via a future, that rclcpp should stop spinning upon + * message handling. + */ +class MetricsMessageSubscriber : public rclcpp::Node, public PromiseSetter +{ +public: + /** + * Constucts a MetricsMessageSubscriber + * \param name the node name + * \param name the topic name + * \param number of messages to receive to trigger the PromiseSetter future + */ + MetricsMessageSubscriber( + const std::string & name, + const std::string & topic_name, + const int number_of_messages_to_receive = 1) + : rclcpp::Node(name), + number_of_messages_to_receive_(number_of_messages_to_receive) + { + auto callback = [this](MetricsMessage::UniquePtr msg) { + this->MetricsMessageCallback(*msg); + }; + subscription_ = create_subscription>( + topic_name, + 0 /*history_depth*/, + callback); + } + + /** + * Acquires a mutex in order to get the last message received member. + * @return the last message received + */ + MetricsMessage GetLastReceivedMessage() const + { + std::unique_lock ulock{mutex_}; + return last_received_message_; + } + + /** + * Return the number of messages received by this subscriber + * @return the number of messages received by the subscriber callback + */ + int GetNumberOfMessagesReceived() const + { + return num_messages_received_; + } + +private: + /** + * Subscriber callback. Acquires a mutex to set the last message received and + * sets the promise to true + * @param msg + */ + void MetricsMessageCallback(const MetricsMessage & msg) + { + std::unique_lock ulock{mutex_}; + ++num_messages_received_; + last_received_message_ = msg; + if (num_messages_received_ >= number_of_messages_to_receive_) { + PromiseSetter::SetPromise(); + } + } + + MetricsMessage last_received_message_; + rclcpp::Subscription::SharedPtr subscription_; + mutable std::mutex mutex_; + std::atomic num_messages_received_{0}; + const int number_of_messages_to_receive_; +}; + +} // namespace topic_statistics +} // namespace rclcpp + +#endif // TOPIC_STATISTICS__TEST_TOPIC_STATS_UTILS_HPP_ From 7c6bff47157d0fcd2f0c1045ce7413815b460cf0 Mon Sep 17 00:00:00 2001 From: Devin Bonnie Date: Mon, 20 Apr 2020 20:12:27 -0700 Subject: [PATCH 17/25] Integrate with Topic Statistics options Fixes after rebasing with master Signed-off-by: Devin Bonnie --- rclcpp/include/rclcpp/create_subscription.hpp | 8 +- rclcpp/include/rclcpp/node_impl.hpp | 31 ++- rclcpp/include/rclcpp/subscription.hpp | 18 +- .../include/rclcpp/subscription_factory.hpp | 10 +- .../subscriber_topic_statistics.hpp | 196 ---------------- .../test_publisher_subscription_count_api.cpp | 4 + .../test_subscription_publisher_count_api.cpp | 4 + .../test_subscriber_topic_statistics.cpp | 217 ------------------ .../test_subscription_topic_statistics.cpp | 124 +++++++++- .../test_topic_stats_utils.hpp | 14 +- 10 files changed, 160 insertions(+), 466 deletions(-) delete mode 100644 rclcpp/include/rclcpp/topic_statistics/subscriber_topic_statistics.hpp delete mode 100644 rclcpp/test/topic_statistics/test_subscriber_topic_statistics.cpp diff --git a/rclcpp/include/rclcpp/create_subscription.hpp b/rclcpp/include/rclcpp/create_subscription.hpp index b41d7f5895..b99146480b 100644 --- a/rclcpp/include/rclcpp/create_subscription.hpp +++ b/rclcpp/include/rclcpp/create_subscription.hpp @@ -24,7 +24,7 @@ #include "rclcpp/subscription_factory.hpp" #include "rclcpp/subscription_options.hpp" #include "rclcpp/qos.hpp" -#include "rclcpp/topic_statistics/subscriber_topic_statistics.hpp" +#include "rclcpp/topic_statistics/subscription_topic_statistics.hpp" #include "rmw/qos_profiles.h" namespace rclcpp @@ -60,8 +60,8 @@ create_subscription( typename MessageMemoryStrategyT::SharedPtr msg_mem_strat = ( MessageMemoryStrategyT::create_default() ), - std::shared_ptr> - subscriber_topic_stats = nullptr + std::shared_ptr> + subscription_topic_stats = nullptr ) { using rclcpp::node_interfaces::get_node_topics_interface; @@ -71,7 +71,7 @@ create_subscription( std::forward(callback), options, msg_mem_strat, - subscriber_topic_stats + subscription_topic_stats ); auto sub = node_topics->create_subscription(topic_name, factory, qos); diff --git a/rclcpp/include/rclcpp/node_impl.hpp b/rclcpp/include/rclcpp/node_impl.hpp index 631e98e288..502581998e 100644 --- a/rclcpp/include/rclcpp/node_impl.hpp +++ b/rclcpp/include/rclcpp/node_impl.hpp @@ -46,7 +46,6 @@ #include "rclcpp/timer.hpp" #include "statistics_msgs/msg/metrics_message.hpp" -#include #ifndef RCLCPP__NODE_HPP_ #include "node.hpp" @@ -96,22 +95,19 @@ Node::create_subscription( const SubscriptionOptionsWithAllocator & options, typename MessageMemoryStrategyT::SharedPtr msg_mem_strat) { - // TODO(dabonnie): only set if the option is set, via https://github.com/ros2/rclcpp/pull/1057 - if (true) { - // TODO(dabonnie): fix QoS, configurable or hardcoded? + if (options.topic_stats_options.state == rclcpp::TopicStatisticsState::ENABLED) { + // TODO(dabonnie): default QoS? same quos as in sub options? std::shared_ptr> publisher = this->create_publisher( - "topic_statistics" + std::to_string(rand()), // for now make unique + options.topic_stats_options.publish_topic, rclcpp::QoS(10)); - // TODO(dabonnie): fix name via https://github.com/ros2/rclcpp/pull/1057 auto sub_topic_stats = std::make_shared< - rclcpp::topic_statistics::SubscriberTopicStatistics + rclcpp::topic_statistics::SubscriptionTopicStatistics >(this->get_name(), publisher); - // TODO(dabonnie): fix hardcoded duration via https://github.com/ros2/rclcpp/pull/1057 auto timer = this->create_wall_timer( - std::chrono::seconds{10}, [sub_topic_stats]() { + options.topic_stats_options.publish_period, [sub_topic_stats]() { sub_topic_stats->publish_message(); }); @@ -125,16 +121,15 @@ Node::create_subscription( options, msg_mem_strat, sub_topic_stats); + } else { + return rclcpp::create_subscription( + *this, + extend_name_with_sub_namespace(topic_name, this->get_sub_namespace()), + qos, + std::forward(callback), + options, + msg_mem_strat); } - - // TODO(dabonnie): fixme - return rclcpp::create_subscription( - *this, - extend_name_with_sub_namespace(topic_name, this->get_sub_namespace()), - qos, - std::forward(callback), - options, - msg_mem_strat); } template diff --git a/rclcpp/include/rclcpp/subscription.hpp b/rclcpp/include/rclcpp/subscription.hpp index 3a2de416e4..2ef349309f 100644 --- a/rclcpp/include/rclcpp/subscription.hpp +++ b/rclcpp/include/rclcpp/subscription.hpp @@ -47,7 +47,7 @@ #include "rclcpp/type_support_decl.hpp" #include "rclcpp/visibility_control.hpp" #include "rclcpp/waitable.hpp" -#include "rclcpp/topic_statistics/subscriber_topic_statistics.hpp" +#include "rclcpp/topic_statistics/subscription_topic_statistics.hpp" #include "tracetools/tracetools.h" namespace rclcpp @@ -76,8 +76,8 @@ class Subscription : public SubscriptionBase using MessageDeleter = allocator::Deleter; using ConstMessageSharedPtr = std::shared_ptr; using MessageUniquePtr = std::unique_ptr; - using SubscriberTopicStatisticsSharedPtr = - std::shared_ptr>; + using SubscriptionTopicStatisticsSharedPtr = + std::shared_ptr>; RCLCPP_SMART_PTR_DEFINITIONS(Subscription) @@ -102,7 +102,7 @@ class Subscription : public SubscriptionBase AnySubscriptionCallback callback, const rclcpp::SubscriptionOptionsWithAllocator & options, typename MessageMemoryStrategyT::SharedPtr message_memory_strategy, - SubscriberTopicStatisticsSharedPtr subscriber_topic_statistics = nullptr) + SubscriptionTopicStatisticsSharedPtr subscription_topic_statistics = nullptr) : SubscriptionBase( node_base, type_support_handle, @@ -184,8 +184,8 @@ class Subscription : public SubscriptionBase this->setup_intra_process(intra_process_subscription_id, ipm); } - if (subscriber_topic_statistics != nullptr) { - this->subscriber_topic_statistics_ = std::move(subscriber_topic_statistics); + if (subscription_topic_statistics != nullptr) { + this->subscription_topic_statistics_ = std::move(subscription_topic_statistics); } TRACEPOINT( @@ -269,11 +269,11 @@ class Subscription : public SubscriptionBase auto typed_message = std::static_pointer_cast(message); any_callback_.dispatch(typed_message, message_info); - if (subscriber_topic_statistics_) { + if (subscription_topic_statistics_) { const auto nanos = std::chrono::time_point_cast( std::chrono::steady_clock::now()); const auto time = rclcpp::Time(nanos.time_since_epoch().count()); - subscriber_topic_statistics_->handle_message(*typed_message, time); + subscription_topic_statistics_->handle_message(*typed_message, time); } } @@ -323,7 +323,7 @@ class Subscription : public SubscriptionBase typename message_memory_strategy::MessageMemoryStrategy::SharedPtr message_memory_strategy_; /// Component which computes and publishes topic statistics for this subscriber - SubscriberTopicStatisticsSharedPtr subscriber_topic_statistics_{nullptr}; + SubscriptionTopicStatisticsSharedPtr subscription_topic_statistics_{nullptr}; }; } // namespace rclcpp diff --git a/rclcpp/include/rclcpp/subscription_factory.hpp b/rclcpp/include/rclcpp/subscription_factory.hpp index 0430ed071d..ffab0724c8 100644 --- a/rclcpp/include/rclcpp/subscription_factory.hpp +++ b/rclcpp/include/rclcpp/subscription_factory.hpp @@ -32,7 +32,7 @@ #include "rclcpp/subscription_options.hpp" #include "rclcpp/subscription_traits.hpp" #include "rclcpp/visibility_control.hpp" -#include "rclcpp/topic_statistics/subscriber_topic_statistics.hpp" +#include "rclcpp/topic_statistics/subscription_topic_statistics.hpp" namespace rclcpp { @@ -80,8 +80,8 @@ create_subscription_factory( CallbackT && callback, const rclcpp::SubscriptionOptionsWithAllocator & options, typename MessageMemoryStrategyT::SharedPtr msg_mem_strat, - std::shared_ptr> - subscriber_topic_stats = nullptr + std::shared_ptr> + subscription_topic_stats = nullptr ) { auto allocator = options.get_allocator(); @@ -92,7 +92,7 @@ create_subscription_factory( SubscriptionFactory factory { // factory function that creates a MessageT specific SubscriptionT - [options, msg_mem_strat, any_subscription_callback, subscriber_topic_stats]( + [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 @@ -109,7 +109,7 @@ create_subscription_factory( any_subscription_callback, options, msg_mem_strat, - subscriber_topic_stats); + 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. diff --git a/rclcpp/include/rclcpp/topic_statistics/subscriber_topic_statistics.hpp b/rclcpp/include/rclcpp/topic_statistics/subscriber_topic_statistics.hpp deleted file mode 100644 index 4eeae0137e..0000000000 --- a/rclcpp/include/rclcpp/topic_statistics/subscriber_topic_statistics.hpp +++ /dev/null @@ -1,196 +0,0 @@ -// Copyright 2020 Amazon.com, Inc. or its affiliates. All Rights Reserved. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef RCLCPP__TOPIC_STATISTICS__SUBSCRIBER_TOPIC_STATISTICS_HPP_ -#define RCLCPP__TOPIC_STATISTICS__SUBSCRIBER_TOPIC_STATISTICS_HPP_ - -#include -#include -#include -#include - -#include "rcl/time.h" -#include "rclcpp/time.hpp" -#include "rclcpp/publisher.hpp" -#include "rclcpp/timer.hpp" - -#include "libstatistics_collector/collector/generate_statistics_message.hpp" -#include "libstatistics_collector/moving_average_statistics/types.hpp" -#include "libstatistics_collector/topic_statistics_collector/constants.hpp" -#include "libstatistics_collector/topic_statistics_collector/received_message_period.hpp" - -#include "statistics_msgs/msg/metrics_message.hpp" - -namespace rclcpp -{ -namespace topic_statistics -{ - -constexpr const char kDefaultPublishTopicName[]{"topic_statistics"}; -constexpr const std::chrono::milliseconds kDefaultPublishingPeriod{std::chrono::minutes(1)}; - -using libstatistics_collector::collector::GenerateStatisticMessage; -using statistics_msgs::msg::MetricsMessage; -using libstatistics_collector::moving_average_statistics::StatisticData; - -/** - * Class used to collect, measure, and publish topic statistics data. Current statistics - * supported for subscribers are received message age and received message period. - * - * \tparam CallbackMessageT the subscribed message type - */ -template -class SubscriberTopicStatistics -{ - using TopicStatsCollector = - libstatistics_collector::topic_statistics_collector::TopicStatisticsCollector< - CallbackMessageT>; - using ReceivedMessagePeriod = - libstatistics_collector::topic_statistics_collector::ReceivedMessagePeriodCollector< - CallbackMessageT>; - -public: - /// Construct a SubscriberTopicStatistics object. - /** - * This object wraps utilities, defined in libstatistics_collector, to collect, - * measure, and publish topic statistics data. - * - * @param node_name the name of the node, which created this instance, in order to denote - * topic source - * @param publisher instance constructed by the node in order to publish statistics data - */ - SubscriberTopicStatistics( - const std::string & node_name, - rclcpp::Publisher::SharedPtr & publisher) - : node_name_(node_name), - publisher_(std::move(publisher)) - { - // TODO(dabonnie): ros-tooling/aws-roadmap/issues/226, received message age - bring_up(); - } - - virtual ~SubscriberTopicStatistics() - { - tear_down(); - } - - /// Handle a message received by the subscription to collect statistics. - /** - * \param received_message the message received by the subscription - * \param now_nanoseconds current time in nanoseconds - */ - virtual void handle_message( - const CallbackMessageT & received_message, - const rclcpp::Time now_nanoseconds) const - { - for (const auto & collector : subscriber_statistics_collectors_) { - collector->OnMessageReceived(received_message, now_nanoseconds.nanoseconds()); - } - } - - /// Return a vector of all the currently collected data - /** - * \return a vector of all the collected data - */ - std::vector get_current_collector_data() const - { - std::vector data; - for (const auto & collector : subscriber_statistics_collectors_) { - data.push_back(collector->GetStatisticsResults()); - } - return data; - } - - /// Set the timer used to publish statistics messages. - /** - * @param measurement_timer the timer to fire the publisher, created by the node - */ - void set_publisher_timer(const rclcpp::TimerBase::SharedPtr & publisher_timer) - { - publisher_timer_ = publisher_timer; - } - - /// Publish a populated MetricsStatisticsMessage - virtual void publish_message() - { - rclcpp::Time window_end{get_current_nanoseconds_since_epoch()}; - - for (auto & collector : subscriber_statistics_collectors_) { - const auto collected_stats = collector->GetStatisticsResults(); - - auto message = libstatistics_collector::collector::GenerateStatisticMessage( - node_name_, - collector->GetMetricName(), - collector->GetMetricUnit(), - window_start_, - window_end, - collected_stats); - publisher_->publish(message); - } - window_start_ = window_end; - } - -private: - /// Construct and start all collectors and set window_start_. - void bring_up() - { - auto received_message_period = std::make_unique(); - received_message_period->Start(); - subscriber_statistics_collectors_.emplace_back(std::move(received_message_period)); - - window_start_ = rclcpp::Time(get_current_nanoseconds_since_epoch()); - } - - /// Stop all collectors, clear measurements, stop publishing timer, and reset publisher. - void tear_down() - { - for (auto & collector : subscriber_statistics_collectors_) { - collector->Stop(); - } - - subscriber_statistics_collectors_.clear(); - - if (publisher_timer_) { - publisher_timer_->cancel(); - publisher_timer_.reset(); - } - - publisher_.reset(); - } - - /// Return the current nanoseconds (count) since epoch. - /** - * \return the current nanoseconds (count) since epoch - */ - int64_t get_current_nanoseconds_since_epoch() - { - const auto now = std::chrono::system_clock::now(); - return std::chrono::duration_cast(now.time_since_epoch()).count(); - } - - /// Collection of statistics collectors - std::vector> subscriber_statistics_collectors_{}; - /// Node name used to generate topic statistics messages to be published - std::string node_name_; - /// Publisher, created by the node, used to publish topic statistics messages - rclcpp::Publisher::SharedPtr publisher_; - /// Timer which fires the publisher - rclcpp::TimerBase::SharedPtr publisher_timer_; - /// The start of the collection window, used in the published topic statistics message - rclcpp::Time window_start_; -}; -} // namespace topic_statistics -} // namespace rclcpp - -#endif // RCLCPP__TOPIC_STATISTICS__SUBSCRIBER_TOPIC_STATISTICS_HPP_ diff --git a/rclcpp/test/test_publisher_subscription_count_api.cpp b/rclcpp/test/test_publisher_subscription_count_api.cpp index c709ff9bd0..8c10f3b4e0 100644 --- a/rclcpp/test/test_publisher_subscription_count_api.cpp +++ b/rclcpp/test/test_publisher_subscription_count_api.cpp @@ -51,6 +51,10 @@ class TestPublisherSubscriptionCount : public ::testing::TestWithParam - -#include -#include -#include -#include -#include - -#include "libstatistics_collector/moving_average_statistics/types.hpp" - -#include "rclcpp/create_publisher.hpp" -#include "rclcpp/node.hpp" -#include "rclcpp/qos.hpp" -#include "rclcpp/rclcpp.hpp" - -#include "rclcpp/topic_statistics/subscriber_topic_statistics.hpp" - -#include "test_topic_stats_utils.hpp" - -#include "statistics_msgs/msg/metrics_message.hpp" -#include "statistics_msgs/msg/statistic_data_point.hpp" -#include "statistics_msgs/msg/statistic_data_type.hpp" - -#include "test_msgs/msg/empty.hpp" - -namespace -{ -constexpr const char kTestPubNodeName[]{"test_pub_stats_node"}; -constexpr const char kTestSubNodeName[]{"test_sub_stats_node"}; -constexpr const char kTestSubStatsTopic[]{"/test_sub_stats_topic"}; -constexpr const char kTestTopicStatisticsTopic[]{"/test_topic_statistics_topic"}; -constexpr const uint64_t kNoSamples{0}; -// TODO(dabonnie) fix when period is configurable -constexpr const std::chrono::seconds kTestDuration{20}; -} // namespace - -using test_msgs::msg::Empty; -using rclcpp::topic_statistics::SubscriberTopicStatistics; -using statistics_msgs::msg::MetricsMessage; -using statistics_msgs::msg::StatisticDataPoint; -using statistics_msgs::msg::StatisticDataType; -/** - * Empty publisher node: used to publish empty messages - */ -class EmptyPublisher : public rclcpp::Node -{ -public: - EmptyPublisher( - const std::string & name, const std::string & topic, - const std::chrono::milliseconds & publish_period = std::chrono::milliseconds{100}) - : Node(name) - { - publisher_ = create_publisher(topic, 10); - publish_timer_ = this->create_wall_timer( - publish_period, [this]() { - this->publish_message(); - }); - } - - int get_number_published() - { - return number_published_.load(); - } - -private: - void publish_message() - { - ++number_published_; - auto msg = Empty{}; - publisher_->publish(msg); - } - - rclcpp::Publisher::SharedPtr publisher_; - std::atomic number_published_{0}; - rclcpp::TimerBase::SharedPtr publish_timer_; -}; - -/** - * Empty subscriber node: used to create subscriber topic statistics requirements - */ -class EmptySubscriber : public rclcpp::Node -{ -public: - EmptySubscriber(const std::string & name, const std::string & topic) - : Node(name) - { - auto callback = [this](Empty::UniquePtr msg) { - this->receive_message(*msg); - }; - subscription_ = create_subscription>( - topic, - rclcpp::QoS(rclcpp::KeepAll()), - callback); - } - -private: - void receive_message(const Empty &) const - { - } - rclcpp::Subscription::SharedPtr subscription_; -}; - -/** - * Test fixture to bring up and teardown rclcpp - */ -class TestSubscriberTopicStatisticsFixture : public ::testing::Test -{ -protected: - void SetUp() - { - rclcpp::init(0 /* argc */, nullptr /* argv */); - empty_subscriber = std::make_shared( - kTestSubNodeName, - kTestSubStatsTopic); - } - - void TearDown() - { - rclcpp::shutdown(); - empty_subscriber.reset(); - } - std::shared_ptr empty_subscriber; -}; - -TEST_F(TestSubscriberTopicStatisticsFixture, test_manual_construction) -{ - // manually create publisher tied to the node - auto topic_stats_publisher = - empty_subscriber->create_publisher( - kTestTopicStatisticsTopic, - 10); - - // construct a separate instance - auto sub_topic_stats = std::make_unique>( - empty_subscriber->get_name(), - topic_stats_publisher); - - using libstatistics_collector::moving_average_statistics::StatisticData; - - // expect no data has been collected / no samples received - for (const auto & data : sub_topic_stats->get_current_collector_data()) { - EXPECT_TRUE(std::isnan(data.average)); - EXPECT_TRUE(std::isnan(data.min)); - EXPECT_TRUE(std::isnan(data.max)); - EXPECT_TRUE(std::isnan(data.standard_deviation)); - EXPECT_EQ(kNoSamples, data.sample_count); - } -} - -TEST_F(TestSubscriberTopicStatisticsFixture, test_receive_single_empty_stats_message) -{ - // create an empty publisher - auto empty_publisher = std::make_shared( - kTestPubNodeName, - kTestSubStatsTopic); - // empty_subscriber has a topic statistics instance as part of its subscription - // this will listen to and generate statistics for the empty message - - // create a listener for topic statistics messages - auto statistics_listener = std::make_shared( - "test_receive_single_empty_stats_message_listener", - "/topic_statistics"); - - rclcpp::executors::SingleThreadedExecutor ex; - ex.add_node(empty_publisher); - ex.add_node(statistics_listener); - ex.add_node(empty_subscriber); - - // spin and get future - ex.spin_until_future_complete( - statistics_listener->GetFuture(), - kTestDuration); - - // compare message counts, sample count should be the same as published and received count - EXPECT_EQ(1, statistics_listener->GetNumberOfMessagesReceived()); - - // check the received message and the data types - const auto received_message = statistics_listener->GetLastReceivedMessage(); - for (const auto & stats_point : received_message.statistics) { - const auto type = stats_point.data_type; - switch (type) { - case StatisticDataType::STATISTICS_DATA_TYPE_SAMPLE_COUNT: - EXPECT_LT(0, stats_point.data) << "unexpected sample count"; - break; - case StatisticDataType::STATISTICS_DATA_TYPE_AVERAGE: - EXPECT_LT(0, stats_point.data) << "unexpected avg"; - break; - case StatisticDataType::STATISTICS_DATA_TYPE_MINIMUM: - EXPECT_LT(0, stats_point.data) << "unexpected min"; - break; - case StatisticDataType::STATISTICS_DATA_TYPE_MAXIMUM: - EXPECT_LT(0, stats_point.data) << "unexpected max"; - break; - case StatisticDataType::STATISTICS_DATA_TYPE_STDDEV: - EXPECT_LT(0, stats_point.data) << "unexpected stddev"; - break; - default: - FAIL() << "received unknown statistics type: " << std::dec << - static_cast(type); - } - } -} diff --git a/rclcpp/test/topic_statistics/test_subscription_topic_statistics.cpp b/rclcpp/test/topic_statistics/test_subscription_topic_statistics.cpp index de90ecb590..a9d40ba842 100644 --- a/rclcpp/test/topic_statistics/test_subscription_topic_statistics.cpp +++ b/rclcpp/test/topic_statistics/test_subscription_topic_statistics.cpp @@ -14,6 +14,9 @@ #include +#include +#include +#include #include #include #include @@ -24,23 +27,34 @@ #include "rclcpp/node.hpp" #include "rclcpp/qos.hpp" #include "rclcpp/rclcpp.hpp" +#include "rclcpp/subscription_options.hpp" #include "rclcpp/topic_statistics/subscription_topic_statistics.hpp" #include "statistics_msgs/msg/metrics_message.hpp" +#include "statistics_msgs/msg/statistic_data_point.hpp" +#include "statistics_msgs/msg/statistic_data_type.hpp" + #include "test_msgs/msg/empty.hpp" +#include "test_topic_stats_utils.hpp" + namespace { -constexpr const char kTestNodeName[]{"test_sub_stats_node"}; +constexpr const char kTestPubNodeName[]{"test_pub_stats_node"}; +constexpr const char kTestSubNodeName[]{"test_sub_stats_node"}; constexpr const char kTestSubStatsTopic[]{"/test_sub_stats_topic"}; constexpr const char kTestTopicStatisticsTopic[]{"/test_topic_statistics_topic"}; constexpr const uint64_t kNoSamples{0}; +constexpr const std::chrono::milliseconds kTestStatsPublishPeriod{4000}; +constexpr const std::chrono::seconds kTestDuration{10}; } // namespace using test_msgs::msg::Empty; -using statistics_msgs::msg::MetricsMessage; using rclcpp::topic_statistics::SubscriptionTopicStatistics; +using statistics_msgs::msg::MetricsMessage; +using statistics_msgs::msg::StatisticDataPoint; +using statistics_msgs::msg::StatisticDataType; using libstatistics_collector::moving_average_statistics::StatisticData; template @@ -63,6 +77,42 @@ class TestSubscriptionTopicStatistics : public SubscriptionTopicStatistics(topic, 10); + publish_timer_ = this->create_wall_timer( + publish_period, [this]() { + this->publish_message(); + }); + } + + int get_number_published() + { + return number_published_.load(); + } + +private: + void publish_message() + { + ++number_published_; + auto msg = Empty{}; + publisher_->publish(msg); + } + + rclcpp::Publisher::SharedPtr publisher_; + std::atomic number_published_{0}; + rclcpp::TimerBase::SharedPtr publish_timer_; +}; + /** * Empty subscriber node: used to create subscriber topic statistics requirements */ @@ -72,6 +122,10 @@ class EmptySubscriber : public rclcpp::Node EmptySubscriber(const std::string & name, const std::string & topic) : Node(name) { + // manually enable topic statistics via options + auto options = rclcpp::SubscriptionOptions(); + options.topic_stats_options.state = rclcpp::TopicStatisticsState::ENABLED; + auto callback = [this](Empty::UniquePtr msg) { this->receive_message(*msg); }; @@ -79,16 +133,14 @@ class EmptySubscriber : public rclcpp::Node std::function>( topic, rclcpp::QoS(rclcpp::KeepAll()), - callback); + callback, + options); } - virtual ~EmptySubscriber() = default; - private: void receive_message(const Empty &) const { } - rclcpp::Subscription::SharedPtr subscription_; }; @@ -102,7 +154,7 @@ class TestSubscriptionTopicStatisticsFixture : public ::testing::Test { rclcpp::init(0 /* argc */, nullptr /* argv */); empty_subscriber = std::make_shared( - kTestNodeName, + kTestSubNodeName, kTestSubStatsTopic); } @@ -122,13 +174,11 @@ TEST_F(TestSubscriptionTopicStatisticsFixture, test_manual_construction) kTestTopicStatisticsTopic, 10); - // construct the instance + // construct a separate instance auto sub_topic_stats = std::make_unique>( empty_subscriber->get_name(), topic_stats_publisher); - using libstatistics_collector::moving_average_statistics::StatisticData; - // expect no data has been collected / no samples received for (const auto & data : sub_topic_stats->get_current_collector_data()) { EXPECT_TRUE(std::isnan(data.average)); @@ -138,3 +188,57 @@ TEST_F(TestSubscriptionTopicStatisticsFixture, test_manual_construction) EXPECT_EQ(kNoSamples, data.sample_count); } } + +TEST_F(TestSubscriptionTopicStatisticsFixture, test_receive_single_empty_stats_message) +{ + // create an empty publisher + auto empty_publisher = std::make_shared( + kTestPubNodeName, + kTestSubStatsTopic); + // empty_subscriber has a topic statistics instance as part of its subscription + // this will listen to and generate statistics for the empty message + + // create a listener for topic statistics messages + auto statistics_listener = std::make_shared( + "test_receive_single_empty_stats_message_listener", + "/statistics"); + + rclcpp::executors::SingleThreadedExecutor ex; + ex.add_node(empty_publisher); + ex.add_node(statistics_listener); + ex.add_node(empty_subscriber); + + // spin and get future + ex.spin_until_future_complete( + statistics_listener->GetFuture(), + kTestDuration); + + // compare message counts, sample count should be the same as published and received count + EXPECT_EQ(1, statistics_listener->GetNumberOfMessagesReceived()); + + // check the received message and the data types + const auto received_message = statistics_listener->GetLastReceivedMessage(); + for (const auto & stats_point : received_message.statistics) { + const auto type = stats_point.data_type; + switch (type) { + case StatisticDataType::STATISTICS_DATA_TYPE_SAMPLE_COUNT: + EXPECT_LT(0, stats_point.data) << "unexpected sample count"; + break; + case StatisticDataType::STATISTICS_DATA_TYPE_AVERAGE: + EXPECT_LT(0, stats_point.data) << "unexpected avg"; + break; + case StatisticDataType::STATISTICS_DATA_TYPE_MINIMUM: + EXPECT_LT(0, stats_point.data) << "unexpected min"; + break; + case StatisticDataType::STATISTICS_DATA_TYPE_MAXIMUM: + EXPECT_LT(0, stats_point.data) << "unexpected max"; + break; + case StatisticDataType::STATISTICS_DATA_TYPE_STDDEV: + EXPECT_LT(0, stats_point.data) << "unexpected stddev"; + break; + default: + FAIL() << "received unknown statistics type: " << std::dec << + static_cast(type); + } + } +} diff --git a/rclcpp/test/topic_statistics/test_topic_stats_utils.hpp b/rclcpp/test/topic_statistics/test_topic_stats_utils.hpp index 704556055b..0140f919e8 100644 --- a/rclcpp/test/topic_statistics/test_topic_stats_utils.hpp +++ b/rclcpp/test/topic_statistics/test_topic_stats_utils.hpp @@ -41,7 +41,7 @@ class PromiseSetter * Reassign the promise member and return it's future. Acquires a mutex in order * to mutate member variables. * - * @return the promise member's future, called upon PeriodicMeasurement + * \return the promise member's future, called upon PeriodicMeasurement */ std::shared_future GetFuture() { @@ -81,7 +81,7 @@ class MetricsMessageSubscriber : public rclcpp::Node, public PromiseSetter { public: /** - * Constucts a MetricsMessageSubscriber + * Constructs a MetricsMessageSubscriber. * \param name the node name * \param name the topic name * \param number of messages to receive to trigger the PromiseSetter future @@ -105,7 +105,7 @@ class MetricsMessageSubscriber : public rclcpp::Node, public PromiseSetter /** * Acquires a mutex in order to get the last message received member. - * @return the last message received + * \return the last message received */ MetricsMessage GetLastReceivedMessage() const { @@ -114,8 +114,8 @@ class MetricsMessageSubscriber : public rclcpp::Node, public PromiseSetter } /** - * Return the number of messages received by this subscriber - * @return the number of messages received by the subscriber callback + * Return the number of messages received by this subscriber. + * \return the number of messages received by the subscriber callback */ int GetNumberOfMessagesReceived() const { @@ -125,8 +125,8 @@ class MetricsMessageSubscriber : public rclcpp::Node, public PromiseSetter private: /** * Subscriber callback. Acquires a mutex to set the last message received and - * sets the promise to true - * @param msg + * sets the promise to true. + * \param msg */ void MetricsMessageCallback(const MetricsMessage & msg) { From d88ec3053dd411e923c804adb8317328f42fdf61 Mon Sep 17 00:00:00 2001 From: Devin Bonnie Date: Tue, 21 Apr 2020 13:17:33 -0700 Subject: [PATCH 18/25] Update after rebasing Signed-off-by: Devin Bonnie --- .../rclcpp/detail/resolve_enable_topic_statistics.hpp | 1 - rclcpp/include/rclcpp/node_impl.hpp | 5 ++--- .../topic_statistics/test_subscription_topic_statistics.cpp | 2 +- 3 files changed, 3 insertions(+), 5 deletions(-) diff --git a/rclcpp/include/rclcpp/detail/resolve_enable_topic_statistics.hpp b/rclcpp/include/rclcpp/detail/resolve_enable_topic_statistics.hpp index 7c59204ff5..e10a92ef10 100644 --- 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; diff --git a/rclcpp/include/rclcpp/node_impl.hpp b/rclcpp/include/rclcpp/node_impl.hpp index 502581998e..9dce7d2915 100644 --- a/rclcpp/include/rclcpp/node_impl.hpp +++ b/rclcpp/include/rclcpp/node_impl.hpp @@ -95,12 +95,11 @@ Node::create_subscription( const SubscriptionOptionsWithAllocator & options, typename MessageMemoryStrategyT::SharedPtr msg_mem_strat) { - if (options.topic_stats_options.state == rclcpp::TopicStatisticsState::ENABLED) { - // TODO(dabonnie): default QoS? same quos as in sub options? + if (options.topic_stats_options.state == rclcpp::TopicStatisticsState::Enable) { std::shared_ptr> publisher = this->create_publisher( options.topic_stats_options.publish_topic, - rclcpp::QoS(10)); + qos); auto sub_topic_stats = std::make_shared< rclcpp::topic_statistics::SubscriptionTopicStatistics diff --git a/rclcpp/test/topic_statistics/test_subscription_topic_statistics.cpp b/rclcpp/test/topic_statistics/test_subscription_topic_statistics.cpp index a9d40ba842..8076fa98f5 100644 --- a/rclcpp/test/topic_statistics/test_subscription_topic_statistics.cpp +++ b/rclcpp/test/topic_statistics/test_subscription_topic_statistics.cpp @@ -124,7 +124,7 @@ class EmptySubscriber : public rclcpp::Node { // manually enable topic statistics via options auto options = rclcpp::SubscriptionOptions(); - options.topic_stats_options.state = rclcpp::TopicStatisticsState::ENABLED; + options.topic_stats_options.state = rclcpp::TopicStatisticsState::Enable; auto callback = [this](Empty::UniquePtr msg) { this->receive_message(*msg); From 6ae2e93c3f6dcf46ab01c6664068119617f96ade Mon Sep 17 00:00:00 2001 From: Devin Bonnie Date: Tue, 21 Apr 2020 16:38:30 -0700 Subject: [PATCH 19/25] Address minor review comments Signed-off-by: Devin Bonnie --- rclcpp/include/rclcpp/node_interfaces/node_base.hpp | 2 +- rclcpp/include/rclcpp/subscription.hpp | 2 +- rclcpp/test/test_publisher_subscription_count_api.cpp | 4 ---- rclcpp/test/test_subscription_publisher_count_api.cpp | 4 ---- .../test_subscription_topic_statistics.cpp | 7 +++++-- 5 files changed, 7 insertions(+), 12 deletions(-) diff --git a/rclcpp/include/rclcpp/node_interfaces/node_base.hpp b/rclcpp/include/rclcpp/node_interfaces/node_base.hpp index c21514ff97..e271decaef 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_base.hpp +++ b/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 diff --git a/rclcpp/include/rclcpp/subscription.hpp b/rclcpp/include/rclcpp/subscription.hpp index 2ef349309f..a1e544c773 100644 --- a/rclcpp/include/rclcpp/subscription.hpp +++ b/rclcpp/include/rclcpp/subscription.hpp @@ -272,7 +272,7 @@ class Subscription : public SubscriptionBase if (subscription_topic_statistics_) { const auto nanos = std::chrono::time_point_cast( std::chrono::steady_clock::now()); - const auto time = rclcpp::Time(nanos.time_since_epoch().count()); + const auto time = rclcpp::Time(nanos.time_since_epoch().count(), RCL_STEADY_TIME); subscription_topic_statistics_->handle_message(*typed_message, time); } } diff --git a/rclcpp/test/test_publisher_subscription_count_api.cpp b/rclcpp/test/test_publisher_subscription_count_api.cpp index 8c10f3b4e0..c709ff9bd0 100644 --- a/rclcpp/test/test_publisher_subscription_count_api.cpp +++ b/rclcpp/test/test_publisher_subscription_count_api.cpp @@ -51,10 +51,6 @@ class TestPublisherSubscriptionCount : public ::testing::TestWithParam::SharedPtr publisher_; - std::atomic number_published_{0}; + std::atomic number_published_{0}; rclcpp::TimerBase::SharedPtr publish_timer_; }; @@ -136,6 +138,7 @@ class EmptySubscriber : public rclcpp::Node callback, options); } + virtual ~EmptySubscriber() = default; private: void receive_message(const Empty &) const From 80086d0db6ab1ac98016a47d5d71c3f9db7a71f0 Mon Sep 17 00:00:00 2001 From: Devin Bonnie Date: Tue, 21 Apr 2020 19:08:56 -0700 Subject: [PATCH 20/25] Move Topic Statistics instantiation to create_subscription Signed-off-by: Devin Bonnie --- rclcpp/include/rclcpp/create_subscription.hpp | 67 ++++++++++++++++++- rclcpp/include/rclcpp/node_impl.hpp | 44 +++--------- .../test_subscription_topic_statistics.cpp | 2 +- 3 files changed, 74 insertions(+), 39 deletions(-) diff --git a/rclcpp/include/rclcpp/create_subscription.hpp b/rclcpp/include/rclcpp/create_subscription.hpp index b99146480b..1e773f89a7 100644 --- a/rclcpp/include/rclcpp/create_subscription.hpp +++ b/rclcpp/include/rclcpp/create_subscription.hpp @@ -15,16 +15,21 @@ #ifndef RCLCPP__CREATE_SUBSCRIPTION_HPP_ #define RCLCPP__CREATE_SUBSCRIPTION_HPP_ +#include +#include #include #include #include #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 @@ -60,13 +65,43 @@ create_subscription( typename MessageMemoryStrategyT::SharedPtr msg_mem_strat = ( MessageMemoryStrategyT::create_default() ), - std::shared_ptr> - subscription_topic_stats = nullptr + 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(node)); + std::shared_ptr> + subscription_topic_stats = nullptr; + + if (options.topic_stats_options.state == rclcpp::TopicStatisticsState::Enable) { + std::shared_ptr> publisher = + create_publisher( + node, + options.topic_stats_options.publish_topic, + qos); + + subscription_topic_stats = std::make_shared< + rclcpp::topic_statistics::SubscriptionTopicStatistics + >(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( + 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( std::forward(callback), options, @@ -80,6 +115,34 @@ create_subscription( return std::dynamic_pointer_cast(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 rclcpp::WallTimer::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::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_ diff --git a/rclcpp/include/rclcpp/node_impl.hpp b/rclcpp/include/rclcpp/node_impl.hpp index 9dce7d2915..36c0436a05 100644 --- a/rclcpp/include/rclcpp/node_impl.hpp +++ b/rclcpp/include/rclcpp/node_impl.hpp @@ -45,8 +45,6 @@ #include "rclcpp/visibility_control.hpp" #include "rclcpp/timer.hpp" -#include "statistics_msgs/msg/metrics_message.hpp" - #ifndef RCLCPP__NODE_HPP_ #include "node.hpp" #endif @@ -95,40 +93,14 @@ Node::create_subscription( const SubscriptionOptionsWithAllocator & options, typename MessageMemoryStrategyT::SharedPtr msg_mem_strat) { - if (options.topic_stats_options.state == rclcpp::TopicStatisticsState::Enable) { - std::shared_ptr> publisher = - this->create_publisher( - options.topic_stats_options.publish_topic, - qos); - - auto sub_topic_stats = std::make_shared< - rclcpp::topic_statistics::SubscriptionTopicStatistics - >(this->get_name(), publisher); - - auto timer = this->create_wall_timer( - options.topic_stats_options.publish_period, [sub_topic_stats]() { - sub_topic_stats->publish_message(); - }); - - sub_topic_stats->set_publisher_timer(timer); - - return rclcpp::create_subscription( - *this, - extend_name_with_sub_namespace(topic_name, this->get_sub_namespace()), - qos, - std::forward(callback), - options, - msg_mem_strat, - sub_topic_stats); - } else { - return rclcpp::create_subscription( - *this, - extend_name_with_sub_namespace(topic_name, this->get_sub_namespace()), - qos, - std::forward(callback), - options, - msg_mem_strat); - } + return rclcpp::create_subscription( + *this, + extend_name_with_sub_namespace(topic_name, this->get_sub_namespace()), + qos, + std::forward(callback), + options, + msg_mem_strat, + this->get_node_timers_interface()); } template diff --git a/rclcpp/test/topic_statistics/test_subscription_topic_statistics.cpp b/rclcpp/test/topic_statistics/test_subscription_topic_statistics.cpp index 793efd1fdc..416c7fc4ad 100644 --- a/rclcpp/test/topic_statistics/test_subscription_topic_statistics.cpp +++ b/rclcpp/test/topic_statistics/test_subscription_topic_statistics.cpp @@ -46,7 +46,7 @@ constexpr const char kTestSubNodeName[]{"test_sub_stats_node"}; constexpr const char kTestSubStatsTopic[]{"/test_sub_stats_topic"}; constexpr const char kTestTopicStatisticsTopic[]{"/test_topic_statistics_topic"}; constexpr const uint64_t kNoSamples{0}; -constexpr const std::chrono::milliseconds kTestStatsPublishPeriod{4000}; +constexpr const std::chrono::milliseconds kTestStatsPublishPeriod{5000}; constexpr const std::chrono::seconds kTestDuration{10}; } // namespace From 6b9eaa50458d936e439aed9f3f4743dc23421fd9 Mon Sep 17 00:00:00 2001 From: Devin Bonnie Date: Tue, 21 Apr 2020 22:56:22 -0700 Subject: [PATCH 21/25] Fix rebase issue Fix topic statistics enable flag usage Address minor formatting Signed-off-by: Devin Bonnie --- rclcpp/include/rclcpp/create_subscription.hpp | 22 ++++++++++--------- rclcpp/include/rclcpp/node_impl.hpp | 2 +- .../rclcpp/node_interfaces/node_base.hpp | 2 +- 3 files changed, 14 insertions(+), 12 deletions(-) diff --git a/rclcpp/include/rclcpp/create_subscription.hpp b/rclcpp/include/rclcpp/create_subscription.hpp index 1e773f89a7..2fcec03f00 100644 --- a/rclcpp/include/rclcpp/create_subscription.hpp +++ b/rclcpp/include/rclcpp/create_subscription.hpp @@ -21,9 +21,12 @@ #include #include +#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_topics_interface.hpp" #include "rclcpp/node_interfaces/node_timers_interface.hpp" +#include "rclcpp/node_interfaces/node_topics_interface.hpp" #include "rclcpp/subscription_factory.hpp" #include "rclcpp/subscription_options.hpp" @@ -74,7 +77,7 @@ create_subscription( std::shared_ptr> subscription_topic_stats = nullptr; - if (options.topic_stats_options.state == rclcpp::TopicStatisticsState::Enable) { + if (rclcpp::detail::resolve_enable_topic_statistics(options, *node_topics->get_node_base_interface())) { std::shared_ptr> publisher = create_publisher( node, @@ -91,8 +94,7 @@ create_subscription( auto timer = create_timer( std::chrono::duration_cast( - options.topic_stats_options. - publish_period), + options.topic_stats_options.publish_period), sub_call_back, options.callback_group, node_topics->get_node_base_interface(), @@ -122,8 +124,8 @@ create_subscription( * \param nanos period to exectute callback * \param callback * \param group - * \param node_base_ - * \param node_timers_ + * \param node_base + * \param node_timers * \return */ template @@ -132,14 +134,14 @@ 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_) + rclcpp::node_interfaces::NodeBaseInterface * node_base, + const std::shared_ptr & node_timers) { auto timer = rclcpp::WallTimer::make_shared( nanos, std::move(callback), - node_base_->get_context()); - node_timers_->add_timer(timer, group); + node_base->get_context()); + node_timers->add_timer(timer, group); return timer; } diff --git a/rclcpp/include/rclcpp/node_impl.hpp b/rclcpp/include/rclcpp/node_impl.hpp index 36c0436a05..e4fc050881 100644 --- a/rclcpp/include/rclcpp/node_impl.hpp +++ b/rclcpp/include/rclcpp/node_impl.hpp @@ -41,9 +41,9 @@ #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" -#include "rclcpp/timer.hpp" #ifndef RCLCPP__NODE_HPP_ #include "node.hpp" diff --git a/rclcpp/include/rclcpp/node_interfaces/node_base.hpp b/rclcpp/include/rclcpp/node_interfaces/node_base.hpp index e271decaef..c21514ff97 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_base.hpp +++ b/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 = false); + bool enable_topic_statistics_default); RCLCPP_PUBLIC virtual From 5e6974266d378cbd54d5c59540f6172b54a16c9c Mon Sep 17 00:00:00 2001 From: Devin Bonnie Date: Tue, 21 Apr 2020 23:36:15 -0700 Subject: [PATCH 22/25] Move new timer creation method to relevant header Signed-off-by: Devin Bonnie --- rclcpp/include/rclcpp/create_subscription.hpp | 42 ++++--------------- rclcpp/include/rclcpp/create_timer.hpp | 42 +++++++++++++++++++ rclcpp/include/rclcpp/node_impl.hpp | 1 + 3 files changed, 52 insertions(+), 33 deletions(-) diff --git a/rclcpp/include/rclcpp/create_subscription.hpp b/rclcpp/include/rclcpp/create_subscription.hpp index 2fcec03f00..a323946c6e 100644 --- a/rclcpp/include/rclcpp/create_subscription.hpp +++ b/rclcpp/include/rclcpp/create_subscription.hpp @@ -28,11 +28,12 @@ #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/topic_statistics/subscription_topic_statistics.hpp" #include "rclcpp/timer.hpp" +#include "rclcpp/topic_statistics/subscription_topic_statistics.hpp" #include "rmw/qos_profiles.h" namespace rclcpp @@ -77,7 +78,10 @@ create_subscription( std::shared_ptr> subscription_topic_stats = nullptr; - if (rclcpp::detail::resolve_enable_topic_statistics(options, *node_topics->get_node_base_interface())) { + if (rclcpp::detail::resolve_enable_topic_statistics( + options, + *node_topics->get_node_base_interface())) + { std::shared_ptr> publisher = create_publisher( node, @@ -92,13 +96,13 @@ create_subscription( subscription_topic_stats->publish_message(); }; - auto timer = create_timer( + auto timer = create_node_timer( std::chrono::duration_cast( options.topic_stats_options.publish_period), sub_call_back, options.callback_group, node_topics->get_node_base_interface(), - node_timer_interface + node_timer_interface.get() ); subscription_topic_stats->set_publisher_timer(timer); @@ -117,34 +121,6 @@ create_subscription( return std::dynamic_pointer_cast(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 rclcpp::WallTimer::SharedPtr -create_timer( - std::chrono::nanoseconds nanos, - CallbackT callback, - rclcpp::callback_group::CallbackGroup::SharedPtr group, - rclcpp::node_interfaces::NodeBaseInterface * node_base, - const std::shared_ptr & node_timers) -{ - auto timer = rclcpp::WallTimer::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_ diff --git a/rclcpp/include/rclcpp/create_timer.hpp b/rclcpp/include/rclcpp/create_timer.hpp index 208474f345..6b83411a86 100644 --- 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 +#include #include #include #include @@ -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 rclcpp::WallTimer::SharedPtr +create_node_timer( + std::chrono::duration 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::make_shared( + std::chrono::duration_cast(period), + std::move(callback), + node_base->get_context()); + node_timers->add_timer(timer, group); + return timer; +} + } // namespace rclcpp #endif // RCLCPP__CREATE_TIMER_HPP_ diff --git a/rclcpp/include/rclcpp/node_impl.hpp b/rclcpp/include/rclcpp/node_impl.hpp index e4fc050881..c8044bf26a 100644 --- a/rclcpp/include/rclcpp/node_impl.hpp +++ b/rclcpp/include/rclcpp/node_impl.hpp @@ -19,6 +19,7 @@ #include #include +#include #include #include #include From 942d7e0d4983ad1318d1f8f2288c4f5ca08ebd85 Mon Sep 17 00:00:00 2001 From: Devin Bonnie Date: Wed, 22 Apr 2020 01:19:40 -0700 Subject: [PATCH 23/25] Add timers interface to topic interface Signed-off-by: Devin Bonnie --- rclcpp/include/rclcpp/create_subscription.hpp | 7 ++++--- rclcpp/include/rclcpp/node_impl.hpp | 3 +-- .../include/rclcpp/node_interfaces/node_topics.hpp | 10 +++++++++- .../rclcpp/node_interfaces/node_topics_interface.hpp | 6 ++++++ rclcpp/src/rclcpp/node.cpp | 2 +- rclcpp/src/rclcpp/node_interfaces/node_topics.cpp | 12 ++++++++++-- rclcpp_lifecycle/src/lifecycle_node.cpp | 2 +- 7 files changed, 32 insertions(+), 10 deletions(-) diff --git a/rclcpp/include/rclcpp/create_subscription.hpp b/rclcpp/include/rclcpp/create_subscription.hpp index a323946c6e..07bf049eab 100644 --- a/rclcpp/include/rclcpp/create_subscription.hpp +++ b/rclcpp/include/rclcpp/create_subscription.hpp @@ -68,13 +68,14 @@ 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(node)); + auto node_timer_interface = node_topics->get_node_timers_interface(); + std::shared_ptr> subscription_topic_stats = nullptr; @@ -102,7 +103,7 @@ create_subscription( sub_call_back, options.callback_group, node_topics->get_node_base_interface(), - node_timer_interface.get() + node_timer_interface ); subscription_topic_stats->set_publisher_timer(timer); diff --git a/rclcpp/include/rclcpp/node_impl.hpp b/rclcpp/include/rclcpp/node_impl.hpp index c8044bf26a..7f0078c0d9 100644 --- a/rclcpp/include/rclcpp/node_impl.hpp +++ b/rclcpp/include/rclcpp/node_impl.hpp @@ -100,8 +100,7 @@ Node::create_subscription( qos, std::forward(callback), options, - msg_mem_strat, - this->get_node_timers_interface()); + msg_mem_strat); } template diff --git a/rclcpp/include/rclcpp/node_interfaces/node_topics.hpp b/rclcpp/include/rclcpp/node_interfaces/node_topics.hpp index 8209984abc..227d193516 100644 --- 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 @@ class NodeTopics : public NodeTopicsInterface 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 @@ class NodeTopics : public NodeTopicsInterface 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 diff --git a/rclcpp/include/rclcpp/node_interfaces/node_topics_interface.hpp b/rclcpp/include/rclcpp/node_interfaces/node_topics_interface.hpp index 857670c58b..4ba28d5e40 100644 --- 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 @@ class NodeTopicsInterface 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 diff --git a/rclcpp/src/rclcpp/node.cpp b/rclcpp/src/rclcpp/node.cpp index b429b305c2..b73ec6bf2f 100644 --- a/rclcpp/src/rclcpp/node.cpp +++ b/rclcpp/src/rclcpp/node.cpp @@ -108,7 +108,7 @@ Node::Node( node_graph_(new rclcpp::node_interfaces::NodeGraph(node_base_.get())), node_logging_(new rclcpp::node_interfaces::NodeLogging(node_base_.get())), node_timers_(new rclcpp::node_interfaces::NodeTimers(node_base_.get())), - node_topics_(new rclcpp::node_interfaces::NodeTopics(node_base_.get())), + node_topics_(new rclcpp::node_interfaces::NodeTopics(node_base_.get(), node_timers_.get())), node_services_(new rclcpp::node_interfaces::NodeServices(node_base_.get())), node_clock_(new rclcpp::node_interfaces::NodeClock( node_base_, diff --git a/rclcpp/src/rclcpp/node_interfaces/node_topics.cpp b/rclcpp/src/rclcpp/node_interfaces/node_topics.cpp index 11de4a3278..360b4236c3 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_topics.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_topics.cpp @@ -22,8 +22,10 @@ using rclcpp::exceptions::throw_from_rcl_error; using rclcpp::node_interfaces::NodeTopics; -NodeTopics::NodeTopics(rclcpp::node_interfaces::NodeBaseInterface * node_base) -: node_base_(node_base) +NodeTopics::NodeTopics( + rclcpp::node_interfaces::NodeBaseInterface * node_base, + rclcpp::node_interfaces::NodeTimersInterface * node_timers) +: node_base_(node_base), node_timers_(node_timers) {} NodeTopics::~NodeTopics() @@ -121,3 +123,9 @@ NodeTopics::get_node_base_interface() const { return node_base_; } + +rclcpp::node_interfaces::NodeTimersInterface * +NodeTopics::get_node_timers_interface() const +{ + return node_timers_; +} diff --git a/rclcpp_lifecycle/src/lifecycle_node.cpp b/rclcpp_lifecycle/src/lifecycle_node.cpp index 0d6330b4cd..60debba6f7 100644 --- a/rclcpp_lifecycle/src/lifecycle_node.cpp +++ b/rclcpp_lifecycle/src/lifecycle_node.cpp @@ -67,7 +67,7 @@ LifecycleNode::LifecycleNode( node_graph_(new rclcpp::node_interfaces::NodeGraph(node_base_.get())), node_logging_(new rclcpp::node_interfaces::NodeLogging(node_base_.get())), node_timers_(new rclcpp::node_interfaces::NodeTimers(node_base_.get())), - node_topics_(new rclcpp::node_interfaces::NodeTopics(node_base_.get())), + node_topics_(new rclcpp::node_interfaces::NodeTopics(node_base_.get(), node_timers_.get())), node_services_(new rclcpp::node_interfaces::NodeServices(node_base_.get())), node_clock_(new rclcpp::node_interfaces::NodeClock( node_base_, From 4c73fcfa9bb8f4067771d07aadcbe688a3da2f00 Mon Sep 17 00:00:00 2001 From: Devin Bonnie Date: Wed, 22 Apr 2020 01:38:50 -0700 Subject: [PATCH 24/25] Use new create timer method Signed-off-by: Devin Bonnie --- rclcpp/include/rclcpp/node_impl.hpp | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/rclcpp/include/rclcpp/node_impl.hpp b/rclcpp/include/rclcpp/node_impl.hpp index 7f0078c0d9..bcb1419494 100644 --- a/rclcpp/include/rclcpp/node_impl.hpp +++ b/rclcpp/include/rclcpp/node_impl.hpp @@ -38,6 +38,7 @@ #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" @@ -110,12 +111,12 @@ Node::create_wall_timer( CallbackT callback, rclcpp::callback_group::CallbackGroup::SharedPtr group) { - auto timer = rclcpp::WallTimer::make_shared( - std::chrono::duration_cast(period), + return create_node_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 From ec9bb73bc7ebc0f70bc5bfa0221201ff3c843845 Mon Sep 17 00:00:00 2001 From: Devin Bonnie Date: Wed, 22 Apr 2020 10:12:42 -0700 Subject: [PATCH 25/25] Address review comments Signed-off-by: Devin Bonnie --- rclcpp/include/rclcpp/create_subscription.hpp | 6 +++--- rclcpp/include/rclcpp/create_timer.hpp | 2 +- rclcpp/include/rclcpp/node_impl.hpp | 2 +- 3 files changed, 5 insertions(+), 5 deletions(-) diff --git a/rclcpp/include/rclcpp/create_subscription.hpp b/rclcpp/include/rclcpp/create_subscription.hpp index 07bf049eab..207bee4a71 100644 --- a/rclcpp/include/rclcpp/create_subscription.hpp +++ b/rclcpp/include/rclcpp/create_subscription.hpp @@ -74,8 +74,6 @@ create_subscription( using rclcpp::node_interfaces::get_node_topics_interface; auto node_topics = get_node_topics_interface(std::forward(node)); - auto node_timer_interface = node_topics->get_node_timers_interface(); - std::shared_ptr> subscription_topic_stats = nullptr; @@ -97,7 +95,9 @@ create_subscription( subscription_topic_stats->publish_message(); }; - auto timer = create_node_timer( + auto node_timer_interface = node_topics->get_node_timers_interface(); + + auto timer = create_wall_timer( std::chrono::duration_cast( options.topic_stats_options.publish_period), sub_call_back, diff --git a/rclcpp/include/rclcpp/create_timer.hpp b/rclcpp/include/rclcpp/create_timer.hpp index 6b83411a86..15c641af37 100644 --- a/rclcpp/include/rclcpp/create_timer.hpp +++ b/rclcpp/include/rclcpp/create_timer.hpp @@ -87,7 +87,7 @@ create_timer( */ template typename rclcpp::WallTimer::SharedPtr -create_node_timer( +create_wall_timer( std::chrono::duration period, CallbackT callback, rclcpp::callback_group::CallbackGroup::SharedPtr group, diff --git a/rclcpp/include/rclcpp/node_impl.hpp b/rclcpp/include/rclcpp/node_impl.hpp index bcb1419494..512f472f79 100644 --- a/rclcpp/include/rclcpp/node_impl.hpp +++ b/rclcpp/include/rclcpp/node_impl.hpp @@ -111,7 +111,7 @@ Node::create_wall_timer( CallbackT callback, rclcpp::callback_group::CallbackGroup::SharedPtr group) { - return create_node_timer( + return rclcpp::create_wall_timer( period, std::move(callback), group,