From bdec97991096fd76cab0883771c537e9bbb5a76b Mon Sep 17 00:00:00 2001 From: Devin Bonnie Date: Mon, 6 Apr 2020 16:31:50 -0700 Subject: [PATCH 01/15] Add SubscriberTopicStatistics class Signed-off-by: Devin Bonnie --- rclcpp/CMakeLists.txt | 6 + .../subscriber_topic_statistics.hpp | 178 ++++++++++++++++++ rclcpp/package.xml | 2 + 3 files changed, 186 insertions(+) create mode 100644 rclcpp/include/rclcpp/topic_statistics/subscriber_topic_statistics.hpp diff --git a/rclcpp/CMakeLists.txt b/rclcpp/CMakeLists.txt index 781a06cf3b..d32b40cd52 100644 --- a/rclcpp/CMakeLists.txt +++ b/rclcpp/CMakeLists.txt @@ -4,6 +4,8 @@ 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) @@ -116,6 +118,8 @@ add_library(${PROJECT_NAME} ${${PROJECT_NAME}_SRCS}) # specific order: dependents before dependencies ament_target_dependencies(${PROJECT_NAME} + "libstatistics_collector" + "metrics_statistics_msgs" "rcl" "rcl_yaml_param_parser" "rcpputils" @@ -143,6 +147,8 @@ install( ament_export_include_directories(include) ament_export_libraries(${PROJECT_NAME}) +ament_export_dependencies(libstatistics_collector) +ament_export_dependencies(metrics_statistics_msgs) ament_export_dependencies(rcl) ament_export_dependencies(rcpputils) ament_export_dependencies(rcutils) 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_ diff --git a/rclcpp/package.xml b/rclcpp/package.xml index 751450e1d5..2b4807211c 100644 --- a/rclcpp/package.xml +++ b/rclcpp/package.xml @@ -22,6 +22,8 @@ rosidl_typesupport_c rosidl_typesupport_cpp + libstatistics_collector + metrics_statistics_msgs rcl rcl_yaml_param_parser rcpputils From 5f0985798d4775e4f5162200092079e2e5d994aa Mon Sep 17 00:00:00 2001 From: Devin Bonnie Date: Tue, 7 Apr 2020 16:47:01 -0700 Subject: [PATCH 02/15] Add SubscriberTopicStatistics Test Signed-off-by: Devin Bonnie --- rclcpp/CMakeLists.txt | 14 +++ .../subscriber_topic_statistics.hpp | 74 ++++++----- .../test_subscriber_topic_statistics.cpp | 115 ++++++++++++++++++ 3 files changed, 174 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 d32b40cd52..fbdc0d7e2a 100644 --- a/rclcpp/CMakeLists.txt +++ b/rclcpp/CMakeLists.txt @@ -541,6 +541,20 @@ if(BUILD_TESTING) target_link_libraries(test_wait_set ${PROJECT_NAME}) endif() + ament_add_gtest(test_subscriber_topic_statistics test/topic_statistics/test_subscriber_topic_statistics.cpp) + if(TARGET test_subscriber_topic_statistics) + ament_target_dependencies(test_subscriber_topic_statistics + "libstatistics_collector" + "metrics_statistics_msgs" + "rcl_interfaces" + "rcutils" + "rmw" + "rosidl_generator_cpp" + "rosidl_typesupport_cpp" + "test_msgs") + target_link_libraries(test_subscriber_topic_statistics ${PROJECT_NAME}) + endif() + ament_add_gtest(test_subscription_options test/test_subscription_options.cpp) if(TARGET test_subscription_options) ament_target_dependencies(test_subscription_options "rcl") 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 432b4aa6b089d688c2de466e3ead729a5e4b6400 Mon Sep 17 00:00:00 2001 From: Devin Bonnie Date: Wed, 8 Apr 2020 13:47:38 -0700 Subject: [PATCH 03/15] 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 be3fc2c64a2cd87cb28b9f8dbaac16f6a3f30649 Mon Sep 17 00:00:00 2001 From: Devin Bonnie Date: Mon, 13 Apr 2020 14:24:27 -0700 Subject: [PATCH 04/15] 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 2371983160273a01eabbcb3f7571e99c69ca1f4e Mon Sep 17 00:00:00 2001 From: Devin Bonnie Date: Mon, 13 Apr 2020 14:30:49 -0700 Subject: [PATCH 05/15] 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 37ae2116cc80e72424d3919ac6483fd05bcfe3a3 Mon Sep 17 00:00:00 2001 From: Devin Bonnie Date: Mon, 13 Apr 2020 14:35:29 -0700 Subject: [PATCH 06/15] 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 2c2a061db0a4d163c2cae6bf6641c4635f7590ec Mon Sep 17 00:00:00 2001 From: Devin Bonnie Date: Mon, 13 Apr 2020 15:49:56 -0700 Subject: [PATCH 07/15] 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 684901760f05c0114c9659165859793ad8801b1a Mon Sep 17 00:00:00 2001 From: Devin Bonnie Date: Tue, 14 Apr 2020 13:16:15 -0700 Subject: [PATCH 08/15] 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 050c2472be017558a6722878c16e23c5ab059b8a Mon Sep 17 00:00:00 2001 From: Devin Bonnie Date: Tue, 14 Apr 2020 14:01:11 -0700 Subject: [PATCH 09/15] Fix rebase issue Signed-off-by: Devin Bonnie --- rclcpp/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rclcpp/CMakeLists.txt b/rclcpp/CMakeLists.txt index fbdc0d7e2a..6aae8cf81f 100644 --- a/rclcpp/CMakeLists.txt +++ b/rclcpp/CMakeLists.txt @@ -549,7 +549,7 @@ if(BUILD_TESTING) "rcl_interfaces" "rcutils" "rmw" - "rosidl_generator_cpp" + "rosidl_runtime_cpp" "rosidl_typesupport_cpp" "test_msgs") target_link_libraries(test_subscriber_topic_statistics ${PROJECT_NAME}) From d450dfd5bf3cc9322e0d164f106de277185eb5c2 Mon Sep 17 00:00:00 2001 From: Devin Bonnie Date: Tue, 14 Apr 2020 15:13:03 -0700 Subject: [PATCH 10/15] 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 d85862e2352ad5b3bfec72848fdbc7e7f5926f01 Mon Sep 17 00:00:00 2001 From: Devin Bonnie Date: Wed, 15 Apr 2020 00:11:59 -0700 Subject: [PATCH 11/15] 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 0ebb0f738ba3ba0c5e16e5a3eaff76a25ffaf9db Mon Sep 17 00:00:00 2001 From: Devin Bonnie Date: Wed, 15 Apr 2020 11:38:23 -0700 Subject: [PATCH 12/15] 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 60a6ba338ee301bb0b2c3c19cc7d3ab75f864842 Mon Sep 17 00:00:00 2001 From: Devin Bonnie Date: Fri, 17 Apr 2020 14:24:57 -0700 Subject: [PATCH 13/15] Update message dependency Signed-off-by: Devin Bonnie --- rclcpp/CMakeLists.txt | 8 ++++---- .../topic_statistics/subscriber_topic_statistics.hpp | 8 ++++---- rclcpp/package.xml | 2 +- .../topic_statistics/test_subscriber_topic_statistics.cpp | 5 ++--- 4 files changed, 11 insertions(+), 12 deletions(-) diff --git a/rclcpp/CMakeLists.txt b/rclcpp/CMakeLists.txt index 6aae8cf81f..c0a15455bc 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) @@ -16,6 +15,7 @@ find_package(rosgraph_msgs REQUIRED) find_package(rosidl_runtime_cpp REQUIRED) find_package(rosidl_typesupport_c REQUIRED) find_package(rosidl_typesupport_cpp REQUIRED) +find_package(statistics_msgs REQUIRED) find_package(tracetools REQUIRED) # Default to C++14 @@ -119,7 +119,6 @@ add_library(${PROJECT_NAME} # specific order: dependents before dependencies ament_target_dependencies(${PROJECT_NAME} "libstatistics_collector" - "metrics_statistics_msgs" "rcl" "rcl_yaml_param_parser" "rcpputils" @@ -128,6 +127,7 @@ ament_target_dependencies(${PROJECT_NAME} "rosgraph_msgs" "rosidl_typesupport_cpp" "rosidl_runtime_cpp" + "statistics_msgs" "tracetools" ) @@ -148,7 +148,6 @@ ament_export_include_directories(include) ament_export_libraries(${PROJECT_NAME}) ament_export_dependencies(libstatistics_collector) -ament_export_dependencies(metrics_statistics_msgs) ament_export_dependencies(rcl) ament_export_dependencies(rcpputils) ament_export_dependencies(rcutils) @@ -158,6 +157,7 @@ ament_export_dependencies(rosidl_typesupport_cpp) ament_export_dependencies(rosidl_typesupport_c) ament_export_dependencies(rosidl_runtime_cpp) ament_export_dependencies(rcl_yaml_param_parser) +ament_export_dependencies(statistics_msgs) ament_export_dependencies(tracetools) if(BUILD_TESTING) @@ -545,12 +545,12 @@ if(BUILD_TESTING) if(TARGET test_subscriber_topic_statistics) ament_target_dependencies(test_subscriber_topic_statistics "libstatistics_collector" - "metrics_statistics_msgs" "rcl_interfaces" "rcutils" "rmw" "rosidl_runtime_cpp" "rosidl_typesupport_cpp" + "statistics_msgs" "test_msgs") target_link_libraries(test_subscriber_topic_statistics ${PROJECT_NAME}) endif() 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/package.xml b/rclcpp/package.xml index 2b4807211c..a5e09c0f18 100644 --- a/rclcpp/package.xml +++ b/rclcpp/package.xml @@ -23,12 +23,12 @@ rosidl_typesupport_cpp libstatistics_collector - metrics_statistics_msgs rcl rcl_yaml_param_parser rcpputils rcutils rmw + statistics_msgs tracetools ament_cmake_gmock 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 0f5c5ff84543cb27c12d6f685f9a037aa26daf31 Mon Sep 17 00:00:00 2001 From: Devin Bonnie Date: Fri, 17 Apr 2020 20:03:17 -0700 Subject: [PATCH 14/15] Revert constructor changes Signed-off-by: Devin Bonnie --- .../subscriber_topic_statistics.hpp | 104 +++++++++--------- .../test_subscriber_topic_statistics.cpp | 11 +- 2 files changed, 61 insertions(+), 54 deletions(-) diff --git a/rclcpp/include/rclcpp/topic_statistics/subscriber_topic_statistics.hpp b/rclcpp/include/rclcpp/topic_statistics/subscriber_topic_statistics.hpp index 12e071dc6f..d63fe0d7fd 100644 --- a/rclcpp/include/rclcpp/topic_statistics/subscriber_topic_statistics.hpp +++ b/rclcpp/include/rclcpp/topic_statistics/subscriber_topic_statistics.hpp @@ -25,15 +25,13 @@ #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" - #include "rcl/time.h" -#include "rclcpp/create_publisher.hpp" -#include "rclcpp/node.hpp" -#include "rclcpp/publisher.hpp" #include "rclcpp/time.hpp" +#include "rclcpp/publisher.hpp" #include "rclcpp/timer.hpp" +#include "statistics_msgs/msg/metrics_message.hpp" + namespace rclcpp { namespace topic_statistics @@ -63,35 +61,24 @@ 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. + * This class owns the publisher/ + */ SubscriberTopicStatistics( - rclcpp::Node & node, - const std::string & publishing_topic = kDefaultPublishTopicName, - const std::chrono::milliseconds & publishing_period = kDefaultPublishingPeriod) + const std::string & node_name, + rclcpp::Publisher::SharedPtr publisher) + : node_name_(node_name), + publisher_(std::move(publisher)) { - publisher_ = - node.create_publisher( - publishing_topic, - 10); - - auto callback = [this]() - { - this->publish_message(); - }; - - publisher_timer_ = node.create_wall_timer(publishing_period, callback); - - node_name_ = node.get_name(); - + // TODO(dbbonnie): ros-tooling/aws-roadmap/issues/226, received message age + // TODO(dbbonnie) throw if publisher is null bring_up(); } @@ -127,6 +114,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 +163,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..671676a896 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()) { From 19a18bae3c4f1155335559d947709f2c24c08fae Mon Sep 17 00:00:00 2001 From: Devin Bonnie Date: Mon, 20 Apr 2020 13:43:28 -0700 Subject: [PATCH 15/15] Address review comments Signed-off-by: Devin Bonnie --- rclcpp/CMakeLists.txt | 8 +-- ....hpp => subscription_topic_statistics.hpp} | 70 ++++++++++--------- ...=> test_subscription_topic_statistics.cpp} | 33 +++++++-- 3 files changed, 70 insertions(+), 41 deletions(-) rename rclcpp/include/rclcpp/topic_statistics/{subscriber_topic_statistics.hpp => subscription_topic_statistics.hpp} (82%) rename rclcpp/test/topic_statistics/{test_subscriber_topic_statistics.cpp => test_subscription_topic_statistics.cpp} (75%) diff --git a/rclcpp/CMakeLists.txt b/rclcpp/CMakeLists.txt index c0a15455bc..51b028f090 100644 --- a/rclcpp/CMakeLists.txt +++ b/rclcpp/CMakeLists.txt @@ -541,9 +541,9 @@ if(BUILD_TESTING) target_link_libraries(test_wait_set ${PROJECT_NAME}) endif() - ament_add_gtest(test_subscriber_topic_statistics test/topic_statistics/test_subscriber_topic_statistics.cpp) - if(TARGET test_subscriber_topic_statistics) - ament_target_dependencies(test_subscriber_topic_statistics + ament_add_gtest(test_subscription_topic_statistics test/topic_statistics/test_subscription_topic_statistics.cpp) + if(TARGET test_subscription_topic_statistics) + ament_target_dependencies(test_subscription_topic_statistics "libstatistics_collector" "rcl_interfaces" "rcutils" @@ -552,7 +552,7 @@ if(BUILD_TESTING) "rosidl_typesupport_cpp" "statistics_msgs" "test_msgs") - target_link_libraries(test_subscriber_topic_statistics ${PROJECT_NAME}) + target_link_libraries(test_subscription_topic_statistics ${PROJECT_NAME}) endif() ament_add_gtest(test_subscription_options test/test_subscription_options.cpp) diff --git a/rclcpp/include/rclcpp/topic_statistics/subscriber_topic_statistics.hpp b/rclcpp/include/rclcpp/topic_statistics/subscription_topic_statistics.hpp similarity index 82% rename from rclcpp/include/rclcpp/topic_statistics/subscriber_topic_statistics.hpp rename to rclcpp/include/rclcpp/topic_statistics/subscription_topic_statistics.hpp index d63fe0d7fd..711b04998c 100644 --- a/rclcpp/include/rclcpp/topic_statistics/subscriber_topic_statistics.hpp +++ b/rclcpp/include/rclcpp/topic_statistics/subscription_topic_statistics.hpp @@ -12,8 +12,8 @@ // 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_ +#ifndef RCLCPP__TOPIC_STATISTICS__SUBSCRIPTION_TOPIC_STATISTICS_HPP_ +#define RCLCPP__TOPIC_STATISTICS__SUBSCRIPTION_TOPIC_STATISTICS_HPP_ #include #include @@ -37,8 +37,8 @@ namespace rclcpp namespace topic_statistics { -constexpr const char kDefaultPublishTopicName[]{"topic_statistics"}; -constexpr const std::chrono::milliseconds kDefaultPublishingPeriod{std::chrono::minutes(1)}; +constexpr const char kDefaultPublishTopicName[]{"/statistics"}; +constexpr const std::chrono::milliseconds kDefaultPublishingPeriod{std::chrono::seconds(1)}; using libstatistics_collector::collector::GenerateStatisticMessage; using statistics_msgs::msg::MetricsMessage; @@ -51,7 +51,7 @@ using libstatistics_collector::moving_average_statistics::StatisticData; * \tparam CallbackMessageT the subscribed message type */ template -class SubscriberTopicStatistics +class SubscriptionTopicStatistics { using TopicStatsCollector = libstatistics_collector::topic_statistics_collector::TopicStatisticsCollector< @@ -61,28 +61,33 @@ class SubscriberTopicStatistics CallbackMessageT>; public: - /// Construct a SubscriberTopicStatistics object. + /// Construct a SubscriptionTopicStatistics object. /** * This object wraps utilities, defined in libstatistics_collector, to collect, - * measure, and publish topic statistics data. + * measure, and publish topic statistics data. This throws an invalid_argument + * if the input publisher is null. * - * @param node_name the name of the node, which created this instance, in order to denote + * \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. - * This class owns the publisher/ + * \param publisher instance constructed by the node in order to publish statistics data. + * This class owns the publisher. */ - SubscriberTopicStatistics( + SubscriptionTopicStatistics( const std::string & node_name, rclcpp::Publisher::SharedPtr publisher) : node_name_(node_name), publisher_(std::move(publisher)) { // TODO(dbbonnie): ros-tooling/aws-roadmap/issues/226, received message age - // TODO(dbbonnie) throw if publisher is null + + if (nullptr == publisher_) { + throw std::invalid_argument("publisher pointer is nullptr"); + } + bring_up(); } - virtual ~SubscriberTopicStatistics() + virtual ~SubscriptionTopicStatistics() { tear_down(); } @@ -101,29 +106,16 @@ class SubscriberTopicStatistics } } - /// 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 + * \param measurement_timer the timer to fire the publisher, created by the node */ - void set_publisher_timer(const rclcpp::TimerBase::SharedPtr & publisher_timer) + void set_publisher_timer(rclcpp::TimerBase::SharedPtr publisher_timer) { publisher_timer_ = publisher_timer; } - /// Publish a populated MetricsStatisticsMessage + /// Publish a populated MetricsStatisticsMessage. virtual void publish_message() { rclcpp::Time window_end{get_current_nanoseconds_since_epoch()}; @@ -143,6 +135,20 @@ class SubscriberTopicStatistics window_start_ = window_end; } +protected: + /// 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; + } + private: /// Construct and start all collectors and set window_start_. void bring_up() @@ -175,7 +181,7 @@ class SubscriberTopicStatistics /** * \return the current nanoseconds (count) since epoch */ - int64_t get_current_nanoseconds_since_epoch() + int64_t get_current_nanoseconds_since_epoch() const { const auto now = std::chrono::system_clock::now(); return std::chrono::duration_cast(now.time_since_epoch()).count(); @@ -184,7 +190,7 @@ class SubscriberTopicStatistics /// Collection of statistics collectors std::vector> subscriber_statistics_collectors_{}; /// Node name used to generate topic statistics messages to be published - std::string node_name_; + const std::string node_name_; /// Publisher, created by the node, used to publish topic statistics messages rclcpp::Publisher::SharedPtr publisher_; /// Timer which fires the publisher @@ -195,4 +201,4 @@ class SubscriberTopicStatistics } // namespace topic_statistics } // namespace rclcpp -#endif // RCLCPP__TOPIC_STATISTICS__SUBSCRIBER_TOPIC_STATISTICS_HPP_ +#endif // RCLCPP__TOPIC_STATISTICS__SUBSCRIPTION_TOPIC_STATISTICS_HPP_ diff --git a/rclcpp/test/topic_statistics/test_subscriber_topic_statistics.cpp b/rclcpp/test/topic_statistics/test_subscription_topic_statistics.cpp similarity index 75% rename from rclcpp/test/topic_statistics/test_subscriber_topic_statistics.cpp rename to rclcpp/test/topic_statistics/test_subscription_topic_statistics.cpp index 671676a896..de90ecb590 100644 --- a/rclcpp/test/topic_statistics/test_subscriber_topic_statistics.cpp +++ b/rclcpp/test/topic_statistics/test_subscription_topic_statistics.cpp @@ -16,6 +16,7 @@ #include #include +#include #include "libstatistics_collector/moving_average_statistics/types.hpp" @@ -24,7 +25,7 @@ #include "rclcpp/qos.hpp" #include "rclcpp/rclcpp.hpp" -#include "rclcpp/topic_statistics/subscriber_topic_statistics.hpp" +#include "rclcpp/topic_statistics/subscription_topic_statistics.hpp" #include "statistics_msgs/msg/metrics_message.hpp" #include "test_msgs/msg/empty.hpp" @@ -39,9 +40,29 @@ constexpr const uint64_t kNoSamples{0}; using test_msgs::msg::Empty; using statistics_msgs::msg::MetricsMessage; -using rclcpp::topic_statistics::SubscriberTopicStatistics; +using rclcpp::topic_statistics::SubscriptionTopicStatistics; using libstatistics_collector::moving_average_statistics::StatisticData; +template +class TestSubscriptionTopicStatistics : public SubscriptionTopicStatistics +{ +public: + TestSubscriptionTopicStatistics( + const std::string & node_name, + rclcpp::Publisher::SharedPtr publisher) + : SubscriptionTopicStatistics(node_name, publisher) + { + } + + virtual ~TestSubscriptionTopicStatistics() = default; + + /// Exposed for testing + std::vector get_current_collector_data() const + { + return SubscriptionTopicStatistics::get_current_collector_data(); + } +}; + /** * Empty subscriber node: used to create subscriber topic statistics requirements */ @@ -61,6 +82,8 @@ class EmptySubscriber : public rclcpp::Node callback); } + virtual ~EmptySubscriber() = default; + private: void receive_message(const Empty &) const { @@ -72,7 +95,7 @@ class EmptySubscriber : public rclcpp::Node /** * Test fixture to bring up and teardown rclcpp */ -class TestSubscriberTopicStatisticsFixture : public ::testing::Test +class TestSubscriptionTopicStatisticsFixture : public ::testing::Test { protected: void SetUp() @@ -91,7 +114,7 @@ class TestSubscriberTopicStatisticsFixture : public ::testing::Test std::shared_ptr empty_subscriber; }; -TEST_F(TestSubscriberTopicStatisticsFixture, test_manual_construction) +TEST_F(TestSubscriptionTopicStatisticsFixture, test_manual_construction) { // manually create publisher tied to the node auto topic_stats_publisher = @@ -100,7 +123,7 @@ TEST_F(TestSubscriberTopicStatisticsFixture, test_manual_construction) 10); // construct the instance - auto sub_topic_stats = std::make_unique>( + auto sub_topic_stats = std::make_unique>( empty_subscriber->get_name(), topic_stats_publisher);