From 1121d1dc4c64d9eee5761290b2f550b4b0921ee0 Mon Sep 17 00:00:00 2001 From: Michael Orlov Date: Sun, 1 Dec 2024 18:30:38 -0800 Subject: [PATCH] Add debug information for flaky can_record_again_after_stop test Signed-off-by: Michael Orlov --- rosbag2_test_common/CMakeLists.txt | 4 +- .../publication_manager.hpp | 34 ++++++++++++ rosbag2_test_common/package.xml | 2 + .../test/rosbag2_transport/test_record.cpp | 52 ++++++++++++++----- 4 files changed, 78 insertions(+), 14 deletions(-) diff --git a/rosbag2_test_common/CMakeLists.txt b/rosbag2_test_common/CMakeLists.txt index 91400fc1cb..a3a4f59bad 100644 --- a/rosbag2_test_common/CMakeLists.txt +++ b/rosbag2_test_common/CMakeLists.txt @@ -24,6 +24,7 @@ endif() find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(rcutils REQUIRED) +find_package(test_msgs REQUIRED) add_library(${PROJECT_NAME} INTERFACE) target_include_directories(${PROJECT_NAME} INTERFACE @@ -32,6 +33,7 @@ target_include_directories(${PROJECT_NAME} INTERFACE target_link_libraries(${PROJECT_NAME} INTERFACE rclcpp::rclcpp rcutils::rcutils + ${test_msgs_TARGETS} ) install( @@ -48,7 +50,7 @@ endif() ament_python_install_package(${PROJECT_NAME}) -ament_export_dependencies(rclcpp rcutils) +ament_export_dependencies(rclcpp rcutils test_msgs) # Export old-style CMake variables ament_export_include_directories("include/${PROJECT_NAME}") diff --git a/rosbag2_test_common/include/rosbag2_test_common/publication_manager.hpp b/rosbag2_test_common/include/rosbag2_test_common/publication_manager.hpp index 61fb79f808..9f1a03d902 100644 --- a/rosbag2_test_common/include/rosbag2_test_common/publication_manager.hpp +++ b/rosbag2_test_common/include/rosbag2_test_common/publication_manager.hpp @@ -24,6 +24,7 @@ #include #include "rclcpp/rclcpp.hpp" // rclcpp must be included before the Windows specific includes. +#include "test_msgs/msg/basic_types.hpp" using namespace std::chrono_literals; // NOLINT @@ -158,6 +159,39 @@ class PublicationManager std::vector> publishers_; }; +/// \brief Template specialization with test_msgs::msg::BasicTypes. +/// \details Will assign current repetition number to the message->uint64_value and timestamp +/// from steady clock to the message->int64_value for each publishing message. +template<> +void PublicationManager::setup_publisher( + const std::string & topic_name, + const test_msgs::msg::BasicTypes & message, + const size_t repetition, + const rclcpp::QoS & qos, + const std::chrono::milliseconds interval) +{ + auto publisher = pub_node_->create_publisher(topic_name, qos); + + auto publisher_fcn = [publisher, message, repetition, interval](bool verbose = false) { + for (auto i = 0u; i < repetition; ++i) { + if (rclcpp::ok()) { + test_msgs::msg::BasicTypes pub_message = message; + pub_message.uint64_value = i; + pub_message.int64_value = std::chrono::steady_clock::now().time_since_epoch().count(); + publisher->publish(pub_message); + if (verbose) { + RCLCPP_INFO( + rclcpp::get_logger("publication_manager"), + "publish on topic %s", publisher->get_topic_name()); + } + std::this_thread::sleep_for(interval); + } + } + }; + + publishers_.push_back(std::make_pair(publisher, publisher_fcn)); +} + } // namespace rosbag2_test_common #endif // ROSBAG2_TEST_COMMON__PUBLICATION_MANAGER_HPP_ diff --git a/rosbag2_test_common/package.xml b/rosbag2_test_common/package.xml index e1e862c039..bbc998b19a 100644 --- a/rosbag2_test_common/package.xml +++ b/rosbag2_test_common/package.xml @@ -16,9 +16,11 @@ rclcpp rcutils + test_msgs rclcpp rcutils + test_msgs ament_lint_auto ament_lint_common diff --git a/rosbag2_transport/test/rosbag2_transport/test_record.cpp b/rosbag2_transport/test/rosbag2_transport/test_record.cpp index 2e7fc81069..a7e3020733 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_record.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_record.cpp @@ -23,12 +23,12 @@ #include "rclcpp/rclcpp.hpp" #include "rosbag2_test_common/publication_manager.hpp" -#include "rosbag2_test_common/client_manager.hpp" #include "rosbag2_test_common/wait_for.hpp" #include "rosbag2_transport/recorder.hpp" #include "test_msgs/msg/arrays.hpp" +#include "test_msgs/msg/basic_types.hpp" #include "test_msgs/message_fixtures.hpp" #include "rosbag2_storage/qos.hpp" @@ -123,14 +123,18 @@ TEST_F(RecordIntegrationTestFixture, published_messages_from_multiple_topics_are TEST_F(RecordIntegrationTestFixture, can_record_again_after_stop) { - auto string_message = get_messages_strings()[1]; - std::string string_topic = "/string_topic"; + auto basic_type_message = get_messages_basic_types()[0]; + basic_type_message->uint64_value = 5; + basic_type_message->int64_value = -1; + std::string test_topic = "/can_record_again_after_stop_topic"; + const size_t num_messages_to_publish = 2; rosbag2_test_common::PublicationManager pub_manager; - pub_manager.setup_publisher(string_topic, string_message, 2); + pub_manager.setup_publisher( + test_topic, basic_type_message, num_messages_to_publish, rclcpp::QoS{rclcpp::KeepAll()}, 50ms); rosbag2_transport::RecordOptions record_options = - {false, false, false, {string_topic}, {}, {}, {}, {}, {}, "rmw_format", 50ms}; + {false, false, false, {test_topic}, {}, {}, {}, {}, {}, "rmw_format", 50ms}; auto recorder = std::make_shared( std::move(writer_), storage_options_, record_options); recorder->record(); @@ -141,7 +145,7 @@ TEST_F(RecordIntegrationTestFixture, can_record_again_after_stop) auto & writer = recorder->get_writer_handle(); auto & mock_writer = dynamic_cast(writer.get_implementation_handle()); - ASSERT_TRUE(pub_manager.wait_for_matched(string_topic.c_str())); + ASSERT_TRUE(pub_manager.wait_for_matched(test_topic.c_str())); pub_manager.run_publishers(); @@ -152,7 +156,7 @@ TEST_F(RecordIntegrationTestFixture, can_record_again_after_stop) // Record one more time after stop() recorder->record(); - ASSERT_TRUE(pub_manager.wait_for_matched(string_topic.c_str())); + ASSERT_TRUE(pub_manager.wait_for_matched(test_topic.c_str())); pub_manager.run_publishers(); // 4 because we're running recorder->record() and publishers twice @@ -166,6 +170,16 @@ TEST_F(RecordIntegrationTestFixture, can_record_again_after_stop) EXPECT_TRUE(ret) << "failed to capture expected messages in time"; EXPECT_THAT(recorded_messages, SizeIs(expected_messages)); + // Output debug info if test fails + if (recorded_messages.size() != expected_messages) { + for (size_t i = 0; i < recorded_messages.size(); i++) { + std::cerr << "=> recorded_messages[" << i << "].send_timestamp = " << + recorded_messages[i]->send_timestamp << std::endl; + std::cerr << "recorded_messages[" << i << "].recv_timestamp = " << + recorded_messages[i]->recv_timestamp << std::endl; + } + } + auto recorded_topics = mock_writer.get_topics(); EXPECT_THAT(recorded_topics, SizeIs(1)) << "size=" << recorded_topics.size(); if (recorded_topics.size() != 1) { @@ -173,13 +187,25 @@ TEST_F(RecordIntegrationTestFixture, can_record_again_after_stop) std::cerr << "recorded topic name : " << topic.first << std::endl; } } - EXPECT_THAT(recorded_topics.at(string_topic).first.serialization_format, Eq("rmw_format")); + EXPECT_THAT(recorded_topics.at(test_topic).first.serialization_format, Eq("rmw_format")); + + auto basic_type_messages = + filter_messages(recorded_messages, test_topic); + EXPECT_THAT(basic_type_messages, SizeIs(expected_messages)); + + // Output debug info if test fails + if (basic_type_messages.size() != expected_messages) { + for (size_t i = 0; i < basic_type_messages.size(); i++) { + std::cerr << "=> basic_type_messages[" << i << "].uint64_value = " << + basic_type_messages[i]->uint64_value << std::endl; + std::cerr << "basic_type_messages[" << i << "].int64_value = " << + basic_type_messages[i]->int64_value << std::endl; + } + } - auto string_messages = - filter_messages(recorded_messages, string_topic); - EXPECT_THAT(string_messages, SizeIs(4)); - for (const auto & str_msg : string_messages) { - EXPECT_THAT(str_msg->string_value, Eq(string_message->string_value)); + for (size_t i = 0; i < basic_type_messages.size(); i++) { + const uint64_t expected_u64_value = i % num_messages_to_publish; + EXPECT_THAT(basic_type_messages[i]->uint64_value, Eq(expected_u64_value)); } }