diff --git a/rclcpp/include/rclcpp/topic_statistics/subscription_topic_statistics.hpp b/rclcpp/include/rclcpp/topic_statistics/subscription_topic_statistics.hpp index 614577b404..c09be9d800 100644 --- a/rclcpp/include/rclcpp/topic_statistics/subscription_topic_statistics.hpp +++ b/rclcpp/include/rclcpp/topic_statistics/subscription_topic_statistics.hpp @@ -157,11 +157,11 @@ class SubscriptionTopicStatistics /// Construct and start all collectors and set window_start_. void bring_up() { - const auto received_message_age = std::make_unique(); + auto received_message_age = std::make_unique(); received_message_age->Start(); subscriber_statistics_collectors_.emplace_back(std::move(received_message_age)); - const auto received_message_period = std::make_unique(); + auto received_message_period = std::make_unique(); received_message_period->Start(); subscriber_statistics_collectors_.emplace_back(std::move(received_message_period)); diff --git a/rclcpp/test/topic_statistics/test_subscription_topic_statistics.cpp b/rclcpp/test/topic_statistics/test_subscription_topic_statistics.cpp index de90ecb590..39379128c7 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,33 @@ #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::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 +76,39 @@ class TestSubscriptionTopicStatistics : public SubscriptionTopicStatistics(topic, 10); + publish_timer_ = this->create_wall_timer( + publish_period, [this]() { + this->publish_message(); + }); + } + + virtual ~EmptyPublisher() = default; + +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 +118,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::Enable; + auto callback = [this](Empty::UniquePtr msg) { this->receive_message(*msg); }; @@ -79,16 +129,15 @@ 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 +151,7 @@ class TestSubscriptionTopicStatisticsFixture : public ::testing::Test { rclcpp::init(0 /* argc */, nullptr /* argv */); empty_subscriber = std::make_shared( - kTestNodeName, + kTestSubNodeName, kTestSubStatsTopic); } @@ -122,13 +171,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 +185,66 @@ 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", + 2); + + 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(2, statistics_listener->GetNumberOfMessagesReceived()); + + // check the received message and the data types + const auto received_messages = statistics_listener->GetReceivedMessages(); + + EXPECT_EQ(2u, received_messages.size()); + EXPECT_EQ("message_age", received_messages[0].metrics_source); + EXPECT_EQ("message_period", received_messages[1].metrics_source); + + // Check the collected statistics for message period. + // Message age statistics will not be calculated because Empty messages + // don't have a `header` with timestamp. + for (const auto & stats_point : received_messages[1].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 new file mode 100644 index 0000000000..1399abbaff --- /dev/null +++ b/rclcpp/test/topic_statistics/test_topic_stats_utils.hpp @@ -0,0 +1,152 @@ +// 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 + +#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: + /** + * Constructs 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 = 2) + : 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, + 10 /*history_depth*/, + callback); + } + + /** + * Acquires a mutex in order to get the last message received member. + * \return the last message received + */ + std::vector GetReceivedMessages() const + { + std::unique_lock ulock{mutex_}; + return received_messages_; + } + + /** + * 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_; + received_messages_.push_back(msg); + if (num_messages_received_ >= number_of_messages_to_receive_) { + PromiseSetter::SetPromise(); + } + } + + std::vector received_messages_; + 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_