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 416c7fc4ad..80df0a9aaa 100644 --- a/rclcpp/test/topic_statistics/test_subscription_topic_statistics.cpp +++ b/rclcpp/test/topic_statistics/test_subscription_topic_statistics.cpp @@ -18,6 +18,7 @@ #include #include #include +#include #include #include @@ -46,7 +47,6 @@ 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{5000}; constexpr const std::chrono::seconds kTestDuration{10}; } // namespace @@ -97,11 +97,6 @@ class EmptyPublisher : public rclcpp::Node virtual ~EmptyPublisher() = default; - size_t get_number_published() - { - return number_published_.load(); - } - private: void publish_message() { @@ -204,7 +199,8 @@ TEST_F(TestSubscriptionTopicStatisticsFixture, test_receive_single_empty_stats_m // create a listener for topic statistics messages auto statistics_listener = std::make_shared( "test_receive_single_empty_stats_message_listener", - "/statistics"); + "/statistics", + 2); rclcpp::executors::SingleThreadedExecutor ex; ex.add_node(empty_publisher); @@ -217,31 +213,51 @@ TEST_F(TestSubscriptionTopicStatisticsFixture, test_receive_single_empty_stats_m kTestDuration); // compare message counts, sample count should be the same as published and received count - EXPECT_EQ(1, statistics_listener->GetNumberOfMessagesReceived()); + EXPECT_EQ(2, 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); + const auto received_messages = statistics_listener->GetReceivedMessages(); + + EXPECT_EQ(2u, received_messages.size()); + + std::set received_metrics; + for (const auto & msg : received_messages) { + received_metrics.insert(msg.metrics_source); + } + EXPECT_TRUE(received_metrics.find("message_age") != received_metrics.end()); + EXPECT_TRUE(received_metrics.find("message_period") != received_metrics.end()); + + // Check the collected statistics for message period. + // Message age statistics will not be calculated because Empty messages + // don't have a `header` with timestamp. + + // TODO(prajakta-gokhale): Change Empty message type to something with a `header`, + // and have below assertions work for all collectors. + for (const auto & msg : received_messages) { + if (msg.metrics_source == "message_period") { + for (const auto & stats_point : msg.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 0140f919e8..1399abbaff 100644 --- a/rclcpp/test/topic_statistics/test_topic_stats_utils.hpp +++ b/rclcpp/test/topic_statistics/test_topic_stats_utils.hpp @@ -18,6 +18,7 @@ #include #include #include +#include #include "statistics_msgs/msg/metrics_message.hpp" @@ -89,7 +90,7 @@ class MetricsMessageSubscriber : public rclcpp::Node, public PromiseSetter MetricsMessageSubscriber( const std::string & name, const std::string & topic_name, - const int number_of_messages_to_receive = 1) + const int number_of_messages_to_receive = 2) : rclcpp::Node(name), number_of_messages_to_receive_(number_of_messages_to_receive) { @@ -99,7 +100,7 @@ class MetricsMessageSubscriber : public rclcpp::Node, public PromiseSetter subscription_ = create_subscription>( topic_name, - 0 /*history_depth*/, + 10 /*history_depth*/, callback); } @@ -107,10 +108,10 @@ class MetricsMessageSubscriber : public rclcpp::Node, public PromiseSetter * Acquires a mutex in order to get the last message received member. * \return the last message received */ - MetricsMessage GetLastReceivedMessage() const + std::vector GetReceivedMessages() const { std::unique_lock ulock{mutex_}; - return last_received_message_; + return received_messages_; } /** @@ -132,13 +133,13 @@ class MetricsMessageSubscriber : public rclcpp::Node, public PromiseSetter { std::unique_lock ulock{mutex_}; ++num_messages_received_; - last_received_message_ = msg; + received_messages_.push_back(msg); if (num_messages_received_ >= number_of_messages_to_receive_) { PromiseSetter::SetPromise(); } } - MetricsMessage last_received_message_; + std::vector received_messages_; rclcpp::Subscription::SharedPtr subscription_; mutable std::mutex mutex_; std::atomic num_messages_received_{0};