From c25a916ed51fb58590a7b01b6d955dbd8313996f Mon Sep 17 00:00:00 2001 From: Prajakta Gokhale Date: Tue, 21 Apr 2020 14:33:52 -0700 Subject: [PATCH 01/15] Add received message age metric to topic statistics Signed-off-by: Prajakta Gokhale --- .../topic_statistics/subscription_topic_statistics.hpp | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/rclcpp/include/rclcpp/topic_statistics/subscription_topic_statistics.hpp b/rclcpp/include/rclcpp/topic_statistics/subscription_topic_statistics.hpp index 058ed45818..1ccff45610 100644 --- a/rclcpp/include/rclcpp/topic_statistics/subscription_topic_statistics.hpp +++ b/rclcpp/include/rclcpp/topic_statistics/subscription_topic_statistics.hpp @@ -23,6 +23,7 @@ #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_age.hpp" #include "libstatistics_collector/topic_statistics_collector/received_message_period.hpp" #include "rcl/time.h" @@ -56,6 +57,9 @@ class SubscriptionTopicStatistics using TopicStatsCollector = libstatistics_collector::topic_statistics_collector::TopicStatisticsCollector< CallbackMessageT>; + using ReceivedMessageAge = + libstatistics_collector::topic_statistics_collector::ReceivedMessageAgeCollector< + CallbackMessageT>; using ReceivedMessagePeriod = libstatistics_collector::topic_statistics_collector::ReceivedMessagePeriodCollector< CallbackMessageT>; @@ -173,7 +177,11 @@ class SubscriptionTopicStatistics */ void bring_up() { - auto received_message_period = std::make_unique(); + const 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(); received_message_period->Start(); { std::lock_guard lock(mutex_); From fbee7b0573c0dc7c9be6486fd7b0b0b9882c03f8 Mon Sep 17 00:00:00 2001 From: Prajakta Gokhale Date: Wed, 22 Apr 2020 15:05:32 -0700 Subject: [PATCH 02/15] Add unit tests Signed-off-by: Prajakta Gokhale --- .../subscription_topic_statistics.hpp | 4 +- .../test_subscription_topic_statistics.cpp | 75 ++++++++++++------- .../test_topic_stats_utils.hpp | 13 ++-- 3 files changed, 55 insertions(+), 37 deletions(-) diff --git a/rclcpp/include/rclcpp/topic_statistics/subscription_topic_statistics.hpp b/rclcpp/include/rclcpp/topic_statistics/subscription_topic_statistics.hpp index 1ccff45610..37a2804cbb 100644 --- a/rclcpp/include/rclcpp/topic_statistics/subscription_topic_statistics.hpp +++ b/rclcpp/include/rclcpp/topic_statistics/subscription_topic_statistics.hpp @@ -177,11 +177,11 @@ class SubscriptionTopicStatistics */ 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(); { std::lock_guard lock(mutex_); diff --git a/rclcpp/test/topic_statistics/test_subscription_topic_statistics.cpp b/rclcpp/test/topic_statistics/test_subscription_topic_statistics.cpp index 37433f0f2f..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 @@ -96,11 +97,6 @@ class EmptyPublisher : public rclcpp::Node virtual ~EmptyPublisher() = default; - size_t get_number_published() - { - return number_published_.load(); - } - private: void publish_message() { @@ -203,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); @@ -216,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}; From c9305638f9b53565c6a2d1ddf0ba5a46e41602be Mon Sep 17 00:00:00 2001 From: Prajakta Gokhale Date: Wed, 22 Apr 2020 17:44:44 -0700 Subject: [PATCH 03/15] Add IMU messages in unit test Signed-off-by: Prajakta Gokhale --- rclcpp/CMakeLists.txt | 3 + rclcpp/package.xml | 1 + .../test_subscription_topic_statistics.cpp | 143 ++++++++++++++++++ 3 files changed, 147 insertions(+) diff --git a/rclcpp/CMakeLists.txt b/rclcpp/CMakeLists.txt index 518951017b..028491a0cf 100644 --- a/rclcpp/CMakeLists.txt +++ b/rclcpp/CMakeLists.txt @@ -217,6 +217,7 @@ if(BUILD_TESTING) find_package(rmw_implementation_cmake REQUIRED) + find_package(sensor_msgs REQUIRED) find_package(test_msgs REQUIRED) include(cmake/rclcpp_add_build_failure_test.cmake) @@ -614,11 +615,13 @@ if(BUILD_TESTING) if(TARGET test_subscription_topic_statistics) ament_target_dependencies(test_subscription_topic_statistics "libstatistics_collector" + "rcl" "rcl_interfaces" "rcutils" "rmw" "rosidl_runtime_cpp" "rosidl_typesupport_cpp" + "sensor_msgs" "statistics_msgs" "test_msgs") target_link_libraries(test_subscription_topic_statistics ${PROJECT_NAME}) diff --git a/rclcpp/package.xml b/rclcpp/package.xml index a5e09c0f18..4c3fcb6099 100644 --- a/rclcpp/package.xml +++ b/rclcpp/package.xml @@ -37,6 +37,7 @@ ament_lint_common rmw rmw_implementation_cmake + sensor_msgs test_msgs diff --git a/rclcpp/test/topic_statistics/test_subscription_topic_statistics.cpp b/rclcpp/test/topic_statistics/test_subscription_topic_statistics.cpp index 80df0a9aaa..1523ca1ba0 100644 --- a/rclcpp/test/topic_statistics/test_subscription_topic_statistics.cpp +++ b/rclcpp/test/topic_statistics/test_subscription_topic_statistics.cpp @@ -25,6 +25,7 @@ #include "libstatistics_collector/moving_average_statistics/types.hpp" #include "rclcpp/create_publisher.hpp" +#include "rclcpp/clock.hpp" #include "rclcpp/node.hpp" #include "rclcpp/qos.hpp" #include "rclcpp/rclcpp.hpp" @@ -36,6 +37,7 @@ #include "statistics_msgs/msg/statistic_data_point.hpp" #include "statistics_msgs/msg/statistic_data_type.hpp" +#include "sensor_msgs/msg/imu.hpp" #include "test_msgs/msg/empty.hpp" #include "test_topic_stats_utils.hpp" @@ -50,6 +52,7 @@ constexpr const uint64_t kNoSamples{0}; constexpr const std::chrono::seconds kTestDuration{10}; } // namespace +using sensor_msgs::msg::Imu; using test_msgs::msg::Empty; using rclcpp::topic_statistics::SubscriptionTopicStatistics; using statistics_msgs::msg::MetricsMessage; @@ -110,6 +113,41 @@ class EmptyPublisher : public rclcpp::Node rclcpp::TimerBase::SharedPtr publish_timer_; }; +/** + * IMU message publisher node: used to publish IMU messages with `header` value set + */ +class ImuPublisher : public rclcpp::Node +{ +public: + ImuPublisher( + 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(); + }); + } + + virtual ~ImuPublisher() = default; + +private: + void publish_message() + { + ++number_published_; + auto msg = Imu{}; + msg.header.stamp = clock_.now(); + publisher_->publish(msg); + } + + rclcpp::Clock clock_; + rclcpp::Publisher::SharedPtr publisher_; + std::atomic number_published_{0}; + rclcpp::TimerBase::SharedPtr publish_timer_; +}; + /** * Empty subscriber node: used to create subscriber topic statistics requirements */ @@ -142,6 +180,38 @@ class EmptySubscriber : public rclcpp::Node rclcpp::Subscription::SharedPtr subscription_; }; +/** + * IMU subscriber node: used to create subscriber topic statistics requirements + */ +class ImuSubscriber : public rclcpp::Node +{ +public: + ImuSubscriber(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](Imu::UniquePtr msg) { + this->receive_message(*msg); + }; + subscription_ = create_subscription>( + topic, + rclcpp::QoS(rclcpp::KeepAll()), + callback, + options); + } + virtual ~ImuSubscriber() = default; + +private: + void receive_message(const Imu &) const + { + } + rclcpp::Subscription::SharedPtr subscription_; +}; + /** * Test fixture to bring up and teardown rclcpp */ @@ -154,6 +224,9 @@ class TestSubscriptionTopicStatisticsFixture : public ::testing::Test empty_subscriber = std::make_shared( kTestSubNodeName, kTestSubStatsTopic); + imu_subscriber = std::make_shared( + kTestSubNodeName, + kTestSubStatsTopic); } void TearDown() @@ -162,6 +235,7 @@ class TestSubscriptionTopicStatisticsFixture : public ::testing::Test empty_subscriber.reset(); } std::shared_ptr empty_subscriber; + std::shared_ptr imu_subscriber; }; TEST_F(TestSubscriptionTopicStatisticsFixture, test_manual_construction) @@ -261,3 +335,72 @@ TEST_F(TestSubscriptionTopicStatisticsFixture, test_receive_single_empty_stats_m } } } + +TEST_F(TestSubscriptionTopicStatisticsFixture, test_receive_stats_for_message_with_header) +{ + // create an empty publisher + auto imu_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_stats_for_message_with_header", + "/statistics", + 2); + + rclcpp::executors::SingleThreadedExecutor ex; + ex.add_node(imu_publisher); + ex.add_node(statistics_listener); + ex.add_node(imu_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()); + + 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. + + for (const auto & msg : received_messages) { + 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_NE(std::nan(""), stats_point.data) << "unexpected avg"; + break; + case StatisticDataType::STATISTICS_DATA_TYPE_MINIMUM: + EXPECT_NE(std::nan(""), stats_point.data) << "unexpected min"; + break; + case StatisticDataType::STATISTICS_DATA_TYPE_MAXIMUM: + EXPECT_NE(std::nan(""), stats_point.data) << "unexpected max"; + break; + case StatisticDataType::STATISTICS_DATA_TYPE_STDDEV: + EXPECT_NE(std::nan(""), stats_point.data) << "unexpected stddev"; + break; + default: + FAIL() << "received unknown statistics type: " << std::dec << + static_cast(type); + } + } + } +} From 1cba4b572707f18d647db463780411df086ca4b9 Mon Sep 17 00:00:00 2001 From: Devin Bonnie Date: Wed, 22 Apr 2020 18:28:53 -0700 Subject: [PATCH 04/15] Use system time instead of steady time Test received message age stats values are greater than 0 Signed-off-by: Devin Bonnie --- rclcpp/include/rclcpp/subscription.hpp | 4 ++-- .../test_subscription_topic_statistics.cpp | 15 ++++++++------- 2 files changed, 10 insertions(+), 9 deletions(-) diff --git a/rclcpp/include/rclcpp/subscription.hpp b/rclcpp/include/rclcpp/subscription.hpp index 91343aaee0..2b8108fc94 100644 --- a/rclcpp/include/rclcpp/subscription.hpp +++ b/rclcpp/include/rclcpp/subscription.hpp @@ -271,8 +271,8 @@ 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(), RCL_STEADY_TIME); + std::chrono::system_clock::now()); + const auto time = rclcpp::Time(nanos.time_since_epoch().count()); subscription_topic_statistics_->handle_message(*typed_message, time); } } diff --git a/rclcpp/test/topic_statistics/test_subscription_topic_statistics.cpp b/rclcpp/test/topic_statistics/test_subscription_topic_statistics.cpp index 1523ca1ba0..51ff9a20d8 100644 --- a/rclcpp/test/topic_statistics/test_subscription_topic_statistics.cpp +++ b/rclcpp/test/topic_statistics/test_subscription_topic_statistics.cpp @@ -25,7 +25,6 @@ #include "libstatistics_collector/moving_average_statistics/types.hpp" #include "rclcpp/create_publisher.hpp" -#include "rclcpp/clock.hpp" #include "rclcpp/node.hpp" #include "rclcpp/qos.hpp" #include "rclcpp/rclcpp.hpp" @@ -138,11 +137,13 @@ class ImuPublisher : public rclcpp::Node { ++number_published_; auto msg = Imu{}; - msg.header.stamp = clock_.now(); + auto now = this->now(); + auto nanos = now.nanoseconds() - 1000*1000; + msg.header.stamp.sec = static_cast(RCL_NS_TO_S(nanos)); + msg.header.stamp.nanosec = static_cast(nanos % (1000 * 1000 * 1000)); publisher_->publish(msg); } - rclcpp::Clock clock_; rclcpp::Publisher::SharedPtr publisher_; std::atomic number_published_{0}; rclcpp::TimerBase::SharedPtr publish_timer_; @@ -386,16 +387,16 @@ TEST_F(TestSubscriptionTopicStatisticsFixture, test_receive_stats_for_message_wi EXPECT_LT(0, stats_point.data) << "unexpected sample count"; break; case StatisticDataType::STATISTICS_DATA_TYPE_AVERAGE: - EXPECT_NE(std::nan(""), stats_point.data) << "unexpected avg"; + EXPECT_LT(0, stats_point.data) << "unexpected avg"; break; case StatisticDataType::STATISTICS_DATA_TYPE_MINIMUM: - EXPECT_NE(std::nan(""), stats_point.data) << "unexpected min"; + EXPECT_LT(0, stats_point.data) << "unexpected min"; break; case StatisticDataType::STATISTICS_DATA_TYPE_MAXIMUM: - EXPECT_NE(std::nan(""), stats_point.data) << "unexpected max"; + EXPECT_LT(0, stats_point.data) << "unexpected max"; break; case StatisticDataType::STATISTICS_DATA_TYPE_STDDEV: - EXPECT_NE(std::nan(""), stats_point.data) << "unexpected stddev"; + EXPECT_LE(0, stats_point.data) << "unexpected stddev"; break; default: FAIL() << "received unknown statistics type: " << std::dec << From faff0479a1b1d88975301be0a9ac0724ac6cc6ef Mon Sep 17 00:00:00 2001 From: Devin Bonnie Date: Wed, 22 Apr 2020 18:37:11 -0700 Subject: [PATCH 05/15] Fix test warnings Signed-off-by: Devin Bonnie --- .../test_subscription_topic_statistics.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/rclcpp/test/topic_statistics/test_subscription_topic_statistics.cpp b/rclcpp/test/topic_statistics/test_subscription_topic_statistics.cpp index 51ff9a20d8..be04f796dd 100644 --- a/rclcpp/test/topic_statistics/test_subscription_topic_statistics.cpp +++ b/rclcpp/test/topic_statistics/test_subscription_topic_statistics.cpp @@ -225,9 +225,6 @@ class TestSubscriptionTopicStatisticsFixture : public ::testing::Test empty_subscriber = std::make_shared( kTestSubNodeName, kTestSubStatsTopic); - imu_subscriber = std::make_shared( - kTestSubNodeName, - kTestSubStatsTopic); } void TearDown() @@ -236,7 +233,6 @@ class TestSubscriptionTopicStatisticsFixture : public ::testing::Test empty_subscriber.reset(); } std::shared_ptr empty_subscriber; - std::shared_ptr imu_subscriber; }; TEST_F(TestSubscriptionTopicStatisticsFixture, test_manual_construction) @@ -352,6 +348,10 @@ TEST_F(TestSubscriptionTopicStatisticsFixture, test_receive_stats_for_message_wi "/statistics", 2); + auto imu_subscriber = std::make_shared( + kTestSubNodeName, + kTestSubStatsTopic); + rclcpp::executors::SingleThreadedExecutor ex; ex.add_node(imu_publisher); ex.add_node(statistics_listener); From 2b90d726d4197466dc0c1150a189c8b532075cd2 Mon Sep 17 00:00:00 2001 From: Prajakta Gokhale Date: Wed, 22 Apr 2020 21:28:03 -0700 Subject: [PATCH 06/15] Replace IMU messages with new dummy messages Signed-off-by: Prajakta Gokhale --- rclcpp/CMakeLists.txt | 3 -- rclcpp/package.xml | 1 - .../test_subscription_topic_statistics.cpp | 50 +++++++++---------- 3 files changed, 25 insertions(+), 29 deletions(-) diff --git a/rclcpp/CMakeLists.txt b/rclcpp/CMakeLists.txt index 028491a0cf..518951017b 100644 --- a/rclcpp/CMakeLists.txt +++ b/rclcpp/CMakeLists.txt @@ -217,7 +217,6 @@ if(BUILD_TESTING) find_package(rmw_implementation_cmake REQUIRED) - find_package(sensor_msgs REQUIRED) find_package(test_msgs REQUIRED) include(cmake/rclcpp_add_build_failure_test.cmake) @@ -615,13 +614,11 @@ if(BUILD_TESTING) if(TARGET test_subscription_topic_statistics) ament_target_dependencies(test_subscription_topic_statistics "libstatistics_collector" - "rcl" "rcl_interfaces" "rcutils" "rmw" "rosidl_runtime_cpp" "rosidl_typesupport_cpp" - "sensor_msgs" "statistics_msgs" "test_msgs") target_link_libraries(test_subscription_topic_statistics ${PROJECT_NAME}) diff --git a/rclcpp/package.xml b/rclcpp/package.xml index 4c3fcb6099..a5e09c0f18 100644 --- a/rclcpp/package.xml +++ b/rclcpp/package.xml @@ -37,7 +37,6 @@ ament_lint_common rmw rmw_implementation_cmake - sensor_msgs test_msgs diff --git a/rclcpp/test/topic_statistics/test_subscription_topic_statistics.cpp b/rclcpp/test/topic_statistics/test_subscription_topic_statistics.cpp index be04f796dd..7fe44991cc 100644 --- a/rclcpp/test/topic_statistics/test_subscription_topic_statistics.cpp +++ b/rclcpp/test/topic_statistics/test_subscription_topic_statistics.cpp @@ -36,7 +36,7 @@ #include "statistics_msgs/msg/statistic_data_point.hpp" #include "statistics_msgs/msg/statistic_data_type.hpp" -#include "sensor_msgs/msg/imu.hpp" +#include "test_msgs/msg/message_with_header.hpp" #include "test_msgs/msg/empty.hpp" #include "test_topic_stats_utils.hpp" @@ -51,7 +51,7 @@ constexpr const uint64_t kNoSamples{0}; constexpr const std::chrono::seconds kTestDuration{10}; } // namespace -using sensor_msgs::msg::Imu; +using test_msgs::msg::MessageWithHeader; using test_msgs::msg::Empty; using rclcpp::topic_statistics::SubscriptionTopicStatistics; using statistics_msgs::msg::MetricsMessage; @@ -113,38 +113,38 @@ class EmptyPublisher : public rclcpp::Node }; /** - * IMU message publisher node: used to publish IMU messages with `header` value set + * MessageWithHeader publisher node: used to publish MessageWithHeader with `header` value set */ -class ImuPublisher : public rclcpp::Node +class DummyPublisher : public rclcpp::Node { public: - ImuPublisher( + DummyPublisher( 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); + publisher_ = create_publisher(topic, 10); publish_timer_ = this->create_wall_timer( publish_period, [this]() { this->publish_message(); }); } - virtual ~ImuPublisher() = default; + virtual ~DummyPublisher() = default; private: void publish_message() { ++number_published_; - auto msg = Imu{}; + auto msg = MessageWithHeader{}; auto now = this->now(); - auto nanos = now.nanoseconds() - 1000*1000; + auto nanos = now.nanoseconds() - 1000 * 1000; msg.header.stamp.sec = static_cast(RCL_NS_TO_S(nanos)); msg.header.stamp.nanosec = static_cast(nanos % (1000 * 1000 * 1000)); publisher_->publish(msg); } - rclcpp::Publisher::SharedPtr publisher_; + rclcpp::Publisher::SharedPtr publisher_; std::atomic number_published_{0}; rclcpp::TimerBase::SharedPtr publish_timer_; }; @@ -182,35 +182,35 @@ class EmptySubscriber : public rclcpp::Node }; /** - * IMU subscriber node: used to create subscriber topic statistics requirements + * MessageWithHeader subscriber node: used to create subscriber topic statistics requirements */ -class ImuSubscriber : public rclcpp::Node +class DummySubscriber : public rclcpp::Node { public: - ImuSubscriber(const std::string & name, const std::string & topic) + DummySubscriber(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](Imu::UniquePtr msg) { + auto callback = [this](MessageWithHeader::UniquePtr msg) { this->receive_message(*msg); }; - subscription_ = create_subscription>( + subscription_ = create_subscription>( topic, rclcpp::QoS(rclcpp::KeepAll()), callback, options); } - virtual ~ImuSubscriber() = default; + virtual ~DummySubscriber() = default; private: - void receive_message(const Imu &) const + void receive_message(const MessageWithHeader &) const { } - rclcpp::Subscription::SharedPtr subscription_; + rclcpp::Subscription::SharedPtr subscription_; }; /** @@ -258,7 +258,7 @@ TEST_F(TestSubscriptionTopicStatisticsFixture, test_manual_construction) } } -TEST_F(TestSubscriptionTopicStatisticsFixture, test_receive_single_empty_stats_message) +TEST_F(TestSubscriptionTopicStatisticsFixture, test_receive_stats_for_message_no_header) { // create an empty publisher auto empty_publisher = std::make_shared( @@ -335,8 +335,8 @@ TEST_F(TestSubscriptionTopicStatisticsFixture, test_receive_single_empty_stats_m TEST_F(TestSubscriptionTopicStatisticsFixture, test_receive_stats_for_message_with_header) { - // create an empty publisher - auto imu_publisher = std::make_shared( + // create a MessageWithHeader publisher + auto dummy_publisher = std::make_shared( kTestPubNodeName, kTestSubStatsTopic); // empty_subscriber has a topic statistics instance as part of its subscription @@ -348,14 +348,14 @@ TEST_F(TestSubscriptionTopicStatisticsFixture, test_receive_stats_for_message_wi "/statistics", 2); - auto imu_subscriber = std::make_shared( + auto dummy_subscriber = std::make_shared( kTestSubNodeName, kTestSubStatsTopic); rclcpp::executors::SingleThreadedExecutor ex; - ex.add_node(imu_publisher); + ex.add_node(dummy_publisher); ex.add_node(statistics_listener); - ex.add_node(imu_subscriber); + ex.add_node(dummy_subscriber); // spin and get future ex.spin_until_future_complete( From d9186c1f4a9592c03d672860b3ee5107ee91988e Mon Sep 17 00:00:00 2001 From: Prajakta Gokhale Date: Wed, 22 Apr 2020 22:59:24 -0700 Subject: [PATCH 07/15] Remove outdated TODO and unused test variables Signed-off-by: Prajakta Gokhale --- .../topic_statistics/test_subscription_topic_statistics.cpp | 6 ------ 1 file changed, 6 deletions(-) diff --git a/rclcpp/test/topic_statistics/test_subscription_topic_statistics.cpp b/rclcpp/test/topic_statistics/test_subscription_topic_statistics.cpp index 7fe44991cc..43d971adcf 100644 --- a/rclcpp/test/topic_statistics/test_subscription_topic_statistics.cpp +++ b/rclcpp/test/topic_statistics/test_subscription_topic_statistics.cpp @@ -102,13 +102,11 @@ class EmptyPublisher : public rclcpp::Node 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_; }; @@ -135,7 +133,6 @@ class DummyPublisher : public rclcpp::Node private: void publish_message() { - ++number_published_; auto msg = MessageWithHeader{}; auto now = this->now(); auto nanos = now.nanoseconds() - 1000 * 1000; @@ -145,7 +142,6 @@ class DummyPublisher : public rclcpp::Node } rclcpp::Publisher::SharedPtr publisher_; - std::atomic number_published_{0}; rclcpp::TimerBase::SharedPtr publish_timer_; }; @@ -302,8 +298,6 @@ TEST_F(TestSubscriptionTopicStatisticsFixture, test_receive_stats_for_message_no // 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) { From 2e3fa8a64f4f2bc83e299240b8fa62cb948303b1 Mon Sep 17 00:00:00 2001 From: Devin Bonnie Date: Thu, 23 Apr 2020 13:17:10 -0700 Subject: [PATCH 08/15] Address review comments Signed-off-by: Devin Bonnie --- .../test_subscription_topic_statistics.cpp | 69 +++++++++---------- 1 file changed, 34 insertions(+), 35 deletions(-) diff --git a/rclcpp/test/topic_statistics/test_subscription_topic_statistics.cpp b/rclcpp/test/topic_statistics/test_subscription_topic_statistics.cpp index 43d971adcf..4ac8ed5717 100644 --- a/rclcpp/test/topic_statistics/test_subscription_topic_statistics.cpp +++ b/rclcpp/test/topic_statistics/test_subscription_topic_statistics.cpp @@ -113,10 +113,10 @@ class EmptyPublisher : public rclcpp::Node /** * MessageWithHeader publisher node: used to publish MessageWithHeader with `header` value set */ -class DummyPublisher : public rclcpp::Node +class MessageWithHeaderPublisher : public rclcpp::Node { public: - DummyPublisher( + MessageWithHeaderPublisher( const std::string & name, const std::string & topic, const std::chrono::milliseconds & publish_period = std::chrono::milliseconds{100}) : Node(name) @@ -128,7 +128,7 @@ class DummyPublisher : public rclcpp::Node }); } - virtual ~DummyPublisher() = default; + virtual ~MessageWithHeaderPublisher() = default; private: void publish_message() @@ -180,10 +180,10 @@ class EmptySubscriber : public rclcpp::Node /** * MessageWithHeader subscriber node: used to create subscriber topic statistics requirements */ -class DummySubscriber : public rclcpp::Node +class MessageWithHeaderSubscriber : public rclcpp::Node { public: - DummySubscriber(const std::string & name, const std::string & topic) + MessageWithHeaderSubscriber(const std::string & name, const std::string & topic) : Node(name) { // manually enable topic statistics via options @@ -200,7 +200,7 @@ class DummySubscriber : public rclcpp::Node callback, options); } - virtual ~DummySubscriber() = default; + virtual ~MessageWithHeaderSubscriber() = default; private: void receive_message(const MessageWithHeader &) const @@ -297,31 +297,31 @@ TEST_F(TestSubscriptionTopicStatisticsFixture, test_receive_stats_for_message_no // 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 & 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); - } + if (msg.metrics_source != "message_period") { + continue; + } + 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); } } } @@ -330,7 +330,7 @@ TEST_F(TestSubscriptionTopicStatisticsFixture, test_receive_stats_for_message_no TEST_F(TestSubscriptionTopicStatisticsFixture, test_receive_stats_for_message_with_header) { // create a MessageWithHeader publisher - auto dummy_publisher = std::make_shared( + auto msg_with_header_publisher = std::make_shared( kTestPubNodeName, kTestSubStatsTopic); // empty_subscriber has a topic statistics instance as part of its subscription @@ -342,14 +342,14 @@ TEST_F(TestSubscriptionTopicStatisticsFixture, test_receive_stats_for_message_wi "/statistics", 2); - auto dummy_subscriber = std::make_shared( + auto msg_with_header_subscriber = std::make_shared( kTestSubNodeName, kTestSubStatsTopic); rclcpp::executors::SingleThreadedExecutor ex; - ex.add_node(dummy_publisher); + ex.add_node(msg_with_header_publisher); ex.add_node(statistics_listener); - ex.add_node(dummy_subscriber); + ex.add_node(msg_with_header_subscriber); // spin and get future ex.spin_until_future_complete( @@ -372,7 +372,6 @@ TEST_F(TestSubscriptionTopicStatisticsFixture, test_receive_stats_for_message_wi EXPECT_TRUE(received_metrics.find("message_period") != received_metrics.end()); // Check the collected statistics for message period. - for (const auto & msg : received_messages) { for (const auto & stats_point : msg.statistics) { const auto type = stats_point.data_type; From 0d9ecf46940bce07ade4fa851b59f8fb4c03ebb7 Mon Sep 17 00:00:00 2001 From: Prajakta Gokhale Date: Thu, 23 Apr 2020 13:24:44 -0700 Subject: [PATCH 09/15] Address review comments Signed-off-by: Prajakta Gokhale --- .../test_subscription_topic_statistics.cpp | 46 ++++++++----------- 1 file changed, 19 insertions(+), 27 deletions(-) diff --git a/rclcpp/test/topic_statistics/test_subscription_topic_statistics.cpp b/rclcpp/test/topic_statistics/test_subscription_topic_statistics.cpp index 4ac8ed5717..8fc591124c 100644 --- a/rclcpp/test/topic_statistics/test_subscription_topic_statistics.cpp +++ b/rclcpp/test/topic_statistics/test_subscription_topic_statistics.cpp @@ -134,10 +134,8 @@ class MessageWithHeaderPublisher : public rclcpp::Node void publish_message() { auto msg = MessageWithHeader{}; - auto now = this->now(); - auto nanos = now.nanoseconds() - 1000 * 1000; - msg.header.stamp.sec = static_cast(RCL_NS_TO_S(nanos)); - msg.header.stamp.nanosec = static_cast(nanos % (1000 * 1000 * 1000)); + // Subtract 1 sec from current time so the received message age is always > 0 + msg.header.stamp = this->now() - rclcpp::Duration{1, 0}; publisher_->publish(msg); } @@ -158,8 +156,8 @@ class EmptySubscriber : public rclcpp::Node auto options = rclcpp::SubscriptionOptions(); options.topic_stats_options.state = rclcpp::TopicStatisticsState::Enable; - auto callback = [this](Empty::UniquePtr msg) { - this->receive_message(*msg); + auto callback = [](Empty::UniquePtr msg) { + (void) msg; }; subscription_ = create_subscription>( @@ -171,9 +169,6 @@ class EmptySubscriber : public rclcpp::Node virtual ~EmptySubscriber() = default; private: - void receive_message(const Empty &) const - { - } rclcpp::Subscription::SharedPtr subscription_; }; @@ -190,8 +185,8 @@ class MessageWithHeaderSubscriber : public rclcpp::Node auto options = rclcpp::SubscriptionOptions(); options.topic_stats_options.state = rclcpp::TopicStatisticsState::Enable; - auto callback = [this](MessageWithHeader::UniquePtr msg) { - this->receive_message(*msg); + auto callback = [](MessageWithHeader::UniquePtr msg) { + (void) msg; }; subscription_ = create_subscription>( @@ -203,9 +198,6 @@ class MessageWithHeaderSubscriber : public rclcpp::Node virtual ~MessageWithHeaderSubscriber() = default; private: - void receive_message(const MessageWithHeader &) const - { - } rclcpp::Subscription::SharedPtr subscription_; }; @@ -233,18 +225,18 @@ class TestSubscriptionTopicStatisticsFixture : public ::testing::Test TEST_F(TestSubscriptionTopicStatisticsFixture, test_manual_construction) { - // manually create publisher tied to the node + // Manually create publisher tied to the node auto topic_stats_publisher = empty_subscriber->create_publisher( kTestTopicStatisticsTopic, 10); - // construct a separate instance + // Construct a separate instance auto sub_topic_stats = std::make_unique>( empty_subscriber->get_name(), topic_stats_publisher); - // expect no data has been collected / no samples received + // 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)); @@ -256,14 +248,14 @@ TEST_F(TestSubscriptionTopicStatisticsFixture, test_manual_construction) TEST_F(TestSubscriptionTopicStatisticsFixture, test_receive_stats_for_message_no_header) { - // create an empty publisher + // 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 + // Create a listener for topic statistics messages auto statistics_listener = std::make_shared( "test_receive_single_empty_stats_message_listener", "/statistics", @@ -274,15 +266,15 @@ TEST_F(TestSubscriptionTopicStatisticsFixture, test_receive_stats_for_message_no ex.add_node(statistics_listener); ex.add_node(empty_subscriber); - // spin and get future + // 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 + // 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 + // Check the received message and the data types const auto received_messages = statistics_listener->GetReceivedMessages(); EXPECT_EQ(2u, received_messages.size()); @@ -329,14 +321,14 @@ TEST_F(TestSubscriptionTopicStatisticsFixture, test_receive_stats_for_message_no TEST_F(TestSubscriptionTopicStatisticsFixture, test_receive_stats_for_message_with_header) { - // create a MessageWithHeader publisher + // Create a MessageWithHeader publisher auto msg_with_header_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 + // Create a listener for topic statistics messages auto statistics_listener = std::make_shared( "test_receive_stats_for_message_with_header", "/statistics", @@ -351,15 +343,15 @@ TEST_F(TestSubscriptionTopicStatisticsFixture, test_receive_stats_for_message_wi ex.add_node(statistics_listener); ex.add_node(msg_with_header_subscriber); - // spin and get future + // 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 + // 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 + // Check the received message and the data types const auto received_messages = statistics_listener->GetReceivedMessages(); EXPECT_EQ(2u, received_messages.size()); From 393a0d33feb502a4f87520984300d11571e24da9 Mon Sep 17 00:00:00 2001 From: Prajakta Gokhale Date: Thu, 23 Apr 2020 15:22:35 -0700 Subject: [PATCH 10/15] Re-add message with header for unit testing Signed-off-by: Prajakta Gokhale --- rclcpp/CMakeLists.txt | 11 +++++++++++ rclcpp/package.xml | 5 ++++- rclcpp/test/msg/Header.msg | 3 +++ rclcpp/test/msg/MessageWithHeader.msg | 3 +++ .../test_subscription_topic_statistics.cpp | 4 ++-- 5 files changed, 23 insertions(+), 3 deletions(-) create mode 100644 rclcpp/test/msg/Header.msg create mode 100644 rclcpp/test/msg/MessageWithHeader.msg diff --git a/rclcpp/CMakeLists.txt b/rclcpp/CMakeLists.txt index 518951017b..5e01bb26cb 100644 --- a/rclcpp/CMakeLists.txt +++ b/rclcpp/CMakeLists.txt @@ -12,6 +12,7 @@ find_package(rcpputils REQUIRED) find_package(rcutils REQUIRED) find_package(rmw REQUIRED) find_package(rosgraph_msgs REQUIRED) +find_package(rosidl_default_generators REQUIRED) find_package(rosidl_runtime_cpp REQUIRED) find_package(rosidl_typesupport_c REQUIRED) find_package(rosidl_typesupport_cpp REQUIRED) @@ -223,6 +224,13 @@ if(BUILD_TESTING) add_definitions(-DTEST_RESOURCES_DIRECTORY="${CMAKE_CURRENT_BINARY_DIR}/test/resources") + rosidl_generate_interfaces(${PROJECT_NAME}_test_msgs + test/msg/Header.msg + test/msg/MessageWithHeader.msg + DEPENDENCIES builtin_interfaces + LIBRARY_NAME ${PROJECT_NAME} + ) + ament_add_gtest(test_client test/test_client.cpp) if(TARGET test_client) ament_target_dependencies(test_client @@ -613,6 +621,7 @@ if(BUILD_TESTING) 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 + "builtin_interfaces" "libstatistics_collector" "rcl_interfaces" "rcutils" @@ -621,9 +630,11 @@ if(BUILD_TESTING) "rosidl_typesupport_cpp" "statistics_msgs" "test_msgs") + rosidl_target_interfaces(test_subscription_topic_statistics ${PROJECT_NAME}_test_msgs "rosidl_typesupport_cpp") target_link_libraries(test_subscription_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/package.xml b/rclcpp/package.xml index a5e09c0f18..9c4b6e2b79 100644 --- a/rclcpp/package.xml +++ b/rclcpp/package.xml @@ -1,6 +1,6 @@ - + rclcpp 0.8.3 The ROS client library in C++. @@ -37,8 +37,11 @@ ament_lint_common rmw rmw_implementation_cmake + rosidl_default_generators test_msgs + rosidl_interface_packages + ament_cmake diff --git a/rclcpp/test/msg/Header.msg b/rclcpp/test/msg/Header.msg new file mode 100644 index 0000000000..bfe5dc5366 --- /dev/null +++ b/rclcpp/test/msg/Header.msg @@ -0,0 +1,3 @@ +# Simple Header message with a timestamp field. + +builtin_interfaces/Time stamp diff --git a/rclcpp/test/msg/MessageWithHeader.msg b/rclcpp/test/msg/MessageWithHeader.msg new file mode 100644 index 0000000000..3bd1b45534 --- /dev/null +++ b/rclcpp/test/msg/MessageWithHeader.msg @@ -0,0 +1,3 @@ +# Message containing a simple Header field. + +Header header diff --git a/rclcpp/test/topic_statistics/test_subscription_topic_statistics.cpp b/rclcpp/test/topic_statistics/test_subscription_topic_statistics.cpp index 8fc591124c..3a7e90c025 100644 --- a/rclcpp/test/topic_statistics/test_subscription_topic_statistics.cpp +++ b/rclcpp/test/topic_statistics/test_subscription_topic_statistics.cpp @@ -25,6 +25,7 @@ #include "libstatistics_collector/moving_average_statistics/types.hpp" #include "rclcpp/create_publisher.hpp" +#include "rclcpp/msg/message_with_header.hpp" #include "rclcpp/node.hpp" #include "rclcpp/qos.hpp" #include "rclcpp/rclcpp.hpp" @@ -36,7 +37,6 @@ #include "statistics_msgs/msg/statistic_data_point.hpp" #include "statistics_msgs/msg/statistic_data_type.hpp" -#include "test_msgs/msg/message_with_header.hpp" #include "test_msgs/msg/empty.hpp" #include "test_topic_stats_utils.hpp" @@ -51,7 +51,7 @@ constexpr const uint64_t kNoSamples{0}; constexpr const std::chrono::seconds kTestDuration{10}; } // namespace -using test_msgs::msg::MessageWithHeader; +using rclcpp::msg::MessageWithHeader; using test_msgs::msg::Empty; using rclcpp::topic_statistics::SubscriptionTopicStatistics; using statistics_msgs::msg::MetricsMessage; From 8cd1361caede7690a7eff5bc87917ae2270fe906 Mon Sep 17 00:00:00 2001 From: Prajakta Gokhale Date: Thu, 23 Apr 2020 16:01:19 -0700 Subject: [PATCH 11/15] Remove extra newline Signed-off-by: Prajakta Gokhale --- rclcpp/CMakeLists.txt | 1 - 1 file changed, 1 deletion(-) diff --git a/rclcpp/CMakeLists.txt b/rclcpp/CMakeLists.txt index 5e01bb26cb..088210a65a 100644 --- a/rclcpp/CMakeLists.txt +++ b/rclcpp/CMakeLists.txt @@ -634,7 +634,6 @@ if(BUILD_TESTING) target_link_libraries(test_subscription_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") From 920c23a964df218575193c0b523bb8c0a4d43b60 Mon Sep 17 00:00:00 2001 From: Devin Bonnie Date: Thu, 23 Apr 2020 16:00:59 -0700 Subject: [PATCH 12/15] Address message review feedback Signed-off-by: Devin Bonnie --- rclcpp/CMakeLists.txt | 1 + rclcpp/package.xml | 4 +--- 2 files changed, 2 insertions(+), 3 deletions(-) diff --git a/rclcpp/CMakeLists.txt b/rclcpp/CMakeLists.txt index 088210a65a..3544c26e6a 100644 --- a/rclcpp/CMakeLists.txt +++ b/rclcpp/CMakeLists.txt @@ -229,6 +229,7 @@ if(BUILD_TESTING) test/msg/MessageWithHeader.msg DEPENDENCIES builtin_interfaces LIBRARY_NAME ${PROJECT_NAME} + SKIP_INSTALL ) ament_add_gtest(test_client test/test_client.cpp) diff --git a/rclcpp/package.xml b/rclcpp/package.xml index 9c4b6e2b79..4e7f15b511 100644 --- a/rclcpp/package.xml +++ b/rclcpp/package.xml @@ -1,6 +1,6 @@ - + rclcpp 0.8.3 The ROS client library in C++. @@ -40,8 +40,6 @@ rosidl_default_generators test_msgs - rosidl_interface_packages - ament_cmake From f43f91e968461c933276cea626c569459622ee4e Mon Sep 17 00:00:00 2001 From: Devin Bonnie Date: Thu, 23 Apr 2020 16:38:48 -0700 Subject: [PATCH 13/15] Address more review feedback Signed-off-by: Devin Bonnie --- rclcpp/CMakeLists.txt | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/rclcpp/CMakeLists.txt b/rclcpp/CMakeLists.txt index 3544c26e6a..2019adf9e5 100644 --- a/rclcpp/CMakeLists.txt +++ b/rclcpp/CMakeLists.txt @@ -619,7 +619,9 @@ if(BUILD_TESTING) target_link_libraries(test_wait_set ${PROJECT_NAME}) endif() - ament_add_gtest(test_subscription_topic_statistics test/topic_statistics/test_subscription_topic_statistics.cpp) + ament_add_gtest(test_subscription_topic_statistics test/topic_statistics/test_subscription_topic_statistics.cpp + APPEND_LIBRARY_DIRS "${CMAKE_CURRENT_BINARY_DIR}" + ) if(TARGET test_subscription_topic_statistics) ament_target_dependencies(test_subscription_topic_statistics "builtin_interfaces" From 92810d03b64ccf965093826657b06fc8979bff25 Mon Sep 17 00:00:00 2001 From: Devin Bonnie Date: Thu, 23 Apr 2020 19:29:54 -0700 Subject: [PATCH 14/15] Fix Windows failure Signed-off-by: Devin Bonnie --- rclcpp/CMakeLists.txt | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/rclcpp/CMakeLists.txt b/rclcpp/CMakeLists.txt index 2019adf9e5..09b34a3b67 100644 --- a/rclcpp/CMakeLists.txt +++ b/rclcpp/CMakeLists.txt @@ -619,8 +619,12 @@ if(BUILD_TESTING) target_link_libraries(test_wait_set ${PROJECT_NAME}) endif() + set(append_library_dirs "${CMAKE_CURRENT_BINARY_DIR}") + if(WIN32) + set(append_library_dirs "${append_library_dirs}/$") + endif() ament_add_gtest(test_subscription_topic_statistics test/topic_statistics/test_subscription_topic_statistics.cpp - APPEND_LIBRARY_DIRS "${CMAKE_CURRENT_BINARY_DIR}" + APPEND_LIBRARY_DIRS "${append_library_dirs}" ) if(TARGET test_subscription_topic_statistics) ament_target_dependencies(test_subscription_topic_statistics From b7e674b5f9bbf8f268766807a9855ff68d7967b1 Mon Sep 17 00:00:00 2001 From: Devin Bonnie Date: Thu, 23 Apr 2020 19:41:35 -0700 Subject: [PATCH 15/15] Only set append_library_dirs once Signed-off-by: Devin Bonnie --- rclcpp/CMakeLists.txt | 4 ---- 1 file changed, 4 deletions(-) diff --git a/rclcpp/CMakeLists.txt b/rclcpp/CMakeLists.txt index 09b34a3b67..ef2f106c34 100644 --- a/rclcpp/CMakeLists.txt +++ b/rclcpp/CMakeLists.txt @@ -619,10 +619,6 @@ if(BUILD_TESTING) target_link_libraries(test_wait_set ${PROJECT_NAME}) endif() - set(append_library_dirs "${CMAKE_CURRENT_BINARY_DIR}") - if(WIN32) - set(append_library_dirs "${append_library_dirs}/$") - endif() ament_add_gtest(test_subscription_topic_statistics test/topic_statistics/test_subscription_topic_statistics.cpp APPEND_LIBRARY_DIRS "${append_library_dirs}" )