diff --git a/rosbag2_compression/test/rosbag2_compression/test_sequential_compression_reader.cpp b/rosbag2_compression/test/rosbag2_compression/test_sequential_compression_reader.cpp index eaef55b8dc..d715c9a8b3 100644 --- a/rosbag2_compression/test/rosbag2_compression/test_sequential_compression_reader.cpp +++ b/rosbag2_compression/test/rosbag2_compression/test_sequential_compression_reader.cpp @@ -54,7 +54,7 @@ class SequentialCompressionReaderTest : public Test rcpputils::fs::remove_all(tmp_dir_); storage_options_.uri = tmp_dir_.string(); topic_with_type_ = rosbag2_storage::TopicMetadata{ - "topic", "test_msgs/BasicTypes", storage_serialization_format_, "", ""}; + "topic", "test_msgs/BasicTypes", storage_serialization_format_, {}, ""}; auto topics_and_types = std::vector{topic_with_type_}; auto message = std::make_shared(); message->topic_name = topic_with_type_.name; diff --git a/rosbag2_compression/test/rosbag2_compression/test_sequential_compression_writer.cpp b/rosbag2_compression/test/rosbag2_compression/test_sequential_compression_writer.cpp index 169afc3056..535ceb35a2 100644 --- a/rosbag2_compression/test/rosbag2_compression/test_sequential_compression_writer.cpp +++ b/rosbag2_compression/test/rosbag2_compression/test_sequential_compression_writer.cpp @@ -243,7 +243,7 @@ TEST_F(SequentialCompressionWriterTest, writer_creates_correct_metadata_relative tmp_dir_storage_options_.max_bagfile_size = 1; writer_->open(tmp_dir_storage_options_); - writer_->create_topic({test_topic_name, test_topic_type, "", "", ""}); + writer_->create_topic({test_topic_name, test_topic_type, "", {}, ""}); auto message = std::make_shared(); message->topic_name = test_topic_name; @@ -285,7 +285,7 @@ TEST_F(SequentialCompressionWriterTest, writer_call_metadata_update_on_open_and_ EXPECT_CALL(*storage_, update_metadata(_)).Times(2); writer_->open(tmp_dir_storage_options_); - writer_->create_topic({test_topic_name, test_topic_type, "", "", ""}); + writer_->create_topic({test_topic_name, test_topic_type, "", {}, ""}); auto message = std::make_shared(); message->topic_name = test_topic_name; @@ -323,7 +323,7 @@ TEST_F(SequentialCompressionWriterTest, writer_call_metadata_update_on_bag_split EXPECT_CALL(*storage_, update_metadata(_)).Times(4); writer_->open(tmp_dir_storage_options_); - writer_->create_topic({test_topic_name, test_topic_type, "", "", ""}); + writer_->create_topic({test_topic_name, test_topic_type, "", {}, ""}); auto message = std::make_shared(); message->topic_name = test_topic_name; @@ -370,7 +370,7 @@ TEST_P(SequentialCompressionWriterTest, writer_writes_with_compression_queue_siz initializeWriter(compression_options); writer_->open(tmp_dir_storage_options_); - writer_->create_topic({test_topic_name, test_topic_type, "", "", ""}); + writer_->create_topic({test_topic_name, test_topic_type, "", {}, ""}); auto message = std::make_shared(); message->topic_name = test_topic_name; diff --git a/rosbag2_cpp/src/rosbag2_cpp/reindexer.cpp b/rosbag2_cpp/src/rosbag2_cpp/reindexer.cpp index 5ae804074e..1a55a0a472 100644 --- a/rosbag2_cpp/src/rosbag2_cpp/reindexer.cpp +++ b/rosbag2_cpp/src/rosbag2_cpp/reindexer.cpp @@ -208,7 +208,7 @@ void Reindexer::aggregate_metadata( ROSBAG2_CPP_LOG_DEBUG_STREAM("Found topic!"); // Merge in the new information found_topic->second.message_count += topic.message_count; - if (topic.topic_metadata.offered_qos_profiles != "") { + if (!topic.topic_metadata.offered_qos_profiles.empty()) { found_topic->second.topic_metadata.offered_qos_profiles = topic.topic_metadata.offered_qos_profiles; } diff --git a/rosbag2_cpp/test/rosbag2_cpp/fake_data.cpp b/rosbag2_cpp/test/rosbag2_cpp/fake_data.cpp index d636a27649..08e4354817 100644 --- a/rosbag2_cpp/test/rosbag2_cpp/fake_data.cpp +++ b/rosbag2_cpp/test/rosbag2_cpp/fake_data.cpp @@ -37,7 +37,7 @@ void write_sample_split_bag( topic_name, "test_msgs/msg/ByteMultiArray", "cdr", - "", + {}, "" }, { diff --git a/rosbag2_cpp/test/rosbag2_cpp/test_info.cpp b/rosbag2_cpp/test/rosbag2_cpp/test_info.cpp index 08b214387b..2ba7701a50 100644 --- a/rosbag2_cpp/test/rosbag2_cpp/test_info.cpp +++ b/rosbag2_cpp/test/rosbag2_cpp/test_info.cpp @@ -86,7 +86,7 @@ TEST_P(ParametrizedTemporaryDirectoryFixture, read_metadata_supports_version_2) { const auto actual_first_topic = metadata.topics_with_message_count[0]; const auto expected_first_topic = - rosbag2_storage::TopicInformation{{"topic1", "type1", "rmw1", "", ""}, 100}; + rosbag2_storage::TopicInformation{{"topic1", "type1", "rmw1", {}, ""}, 100}; EXPECT_EQ(actual_first_topic.topic_metadata, expected_first_topic.topic_metadata); EXPECT_EQ(actual_first_topic.message_count, expected_first_topic.message_count); @@ -95,7 +95,7 @@ TEST_P(ParametrizedTemporaryDirectoryFixture, read_metadata_supports_version_2) { const auto actual_second_topic = metadata.topics_with_message_count[1]; const auto expected_second_topic = - rosbag2_storage::TopicInformation{{"topic2", "type2", "rmw2", "", ""}, 200}; + rosbag2_storage::TopicInformation{{"topic2", "type2", "rmw2", {}, ""}, 200}; EXPECT_EQ(actual_second_topic.topic_metadata, expected_second_topic.topic_metadata); EXPECT_EQ(actual_second_topic.message_count, expected_second_topic.message_count); @@ -121,13 +121,19 @@ TEST_P(ParametrizedTemporaryDirectoryFixture, read_metadata_supports_version_6) " name: topic1\n" " type: type1\n" " serialization_format: rmw1\n" - " offered_qos_profiles: default\n" + " offered_qos_profiles: \"- history: 1\\n depth: 1\\n reliability: 1\\n durability: " + "2\\n deadline:\\n sec: 0\\n nsec: 0\\n lifespan:\\n sec: 0\\n nsec: 0\\n " + "liveliness: 0\\n liveliness_lease_duration:\\n sec: 0\\n nsec: 0\\n " + "avoid_ros_namespace_conventions: false\"\n" " message_count: 100\n" " - topic_metadata:\n" " name: topic2\n" " type: type2\n" " serialization_format: rmw2\n" - " offered_qos_profiles: default\n" + " offered_qos_profiles: \"- history: 1\\n depth: 1\\n reliability: 1\\n durability: " + "2\\n deadline:\\n sec: 0\\n nsec: 0\\n lifespan:\\n sec: 0\\n nsec: 0\\n " + "liveliness: 0\\n liveliness_lease_duration:\\n sec: 0\\n nsec: 0\\n " + "avoid_ros_namespace_conventions: false\"\n" " message_count: 200\n" " compression_format: \"zstd\"\n" " compression_mode: \"FILE\"\n" @@ -186,7 +192,7 @@ TEST_P(ParametrizedTemporaryDirectoryFixture, read_metadata_supports_version_6) { const auto actual_first_topic = metadata.topics_with_message_count[0]; const auto expected_first_topic = - rosbag2_storage::TopicInformation{{"topic1", "type1", "rmw1", "", ""}, 100}; + rosbag2_storage::TopicInformation{{"topic1", "type1", "rmw1", {}, ""}, 100}; EXPECT_EQ(actual_first_topic.topic_metadata, expected_first_topic.topic_metadata); EXPECT_EQ(actual_first_topic.message_count, expected_first_topic.message_count); @@ -195,7 +201,7 @@ TEST_P(ParametrizedTemporaryDirectoryFixture, read_metadata_supports_version_6) { const auto actual_second_topic = metadata.topics_with_message_count[1]; const auto expected_second_topic = - rosbag2_storage::TopicInformation{{"topic2", "type2", "rmw2", "", ""}, 200}; + rosbag2_storage::TopicInformation{{"topic2", "type2", "rmw2", {}, ""}, 200}; EXPECT_EQ(actual_second_topic.topic_metadata, expected_second_topic.topic_metadata); EXPECT_EQ(actual_second_topic.message_count, expected_second_topic.message_count); @@ -256,7 +262,7 @@ TEST_P( EXPECT_THAT(read_metadata.topics_with_message_count, SizeIs(2u)); auto actual_first_topic = read_metadata.topics_with_message_count[0]; rosbag2_storage::TopicInformation expected_first_topic = - {{"topic1", "type1", "rmw1", "", ""}, 100}; + {{"topic1", "type1", "rmw1", {}, ""}, 100}; EXPECT_THAT( actual_first_topic.topic_metadata.name, Eq(expected_first_topic.topic_metadata.name)); @@ -269,7 +275,7 @@ TEST_P( EXPECT_THAT(actual_first_topic.message_count, Eq(expected_first_topic.message_count)); auto actual_second_topic = read_metadata.topics_with_message_count[1]; rosbag2_storage::TopicInformation expected_second_topic = - {{"topic2", "type2", "rmw2", "", ""}, 200}; + {{"topic2", "type2", "rmw2", {}, ""}, 200}; EXPECT_THAT( actual_second_topic.topic_metadata.name, Eq(expected_second_topic.topic_metadata.name)); diff --git a/rosbag2_cpp/test/rosbag2_cpp/test_multifile_reader.cpp b/rosbag2_cpp/test/rosbag2_cpp/test_multifile_reader.cpp index dfd003173f..6537ec6004 100644 --- a/rosbag2_cpp/test/rosbag2_cpp/test_multifile_reader.cpp +++ b/rosbag2_cpp/test/rosbag2_cpp/test_multifile_reader.cpp @@ -53,7 +53,7 @@ class MultifileReaderTest : public Test auto metadata = get_metadata(); auto topic_with_type = rosbag2_storage::TopicMetadata{ - "topic", "test_msgs/BasicTypes", storage_serialization_format_, "", ""}; + "topic", "test_msgs/BasicTypes", storage_serialization_format_, {}, ""}; auto topics_and_types = std::vector{topic_with_type}; metadata.topics_with_message_count.push_back({topic_with_type, 10}); diff --git a/rosbag2_cpp/test/rosbag2_cpp/test_sequential_writer.cpp b/rosbag2_cpp/test/rosbag2_cpp/test_sequential_writer.cpp index 1724cf2497..41871b1ee1 100644 --- a/rosbag2_cpp/test/rosbag2_cpp/test_sequential_writer.cpp +++ b/rosbag2_cpp/test/rosbag2_cpp/test_sequential_writer.cpp @@ -130,7 +130,7 @@ TEST_F( auto message = std::make_shared(); message->topic_name = "test_topic"; writer_->open(storage_options_, {input_format, storage_serialization_format}); - writer_->create_topic({"test_topic", "test_msgs/BasicTypes", "", "", ""}); + writer_->create_topic({"test_topic", "test_msgs/BasicTypes", "", {}, ""}); writer_->write(message); } @@ -147,7 +147,7 @@ TEST_F(SequentialWriterTest, write_does_not_use_converters_if_input_and_output_f auto message = std::make_shared(); message->topic_name = "test_topic"; writer_->open(storage_options_, {storage_serialization_format, storage_serialization_format}); - writer_->create_topic({"test_topic", "test_msgs/BasicTypes", "", "", ""}); + writer_->create_topic({"test_topic", "test_msgs/BasicTypes", "", {}, ""}); writer_->write(message); } @@ -175,7 +175,7 @@ TEST_F(SequentialWriterTest, sequantial_writer_call_metadata_update_on_open_and_ std::string rmw_format = "rmw_format"; writer_->open(storage_options_, {rmw_format, rmw_format}); - writer_->create_topic({test_topic_name, test_topic_type, "", "", ""}); + writer_->create_topic({test_topic_name, test_topic_type, "", {}, ""}); auto message = std::make_shared(); message->topic_name = test_topic_name; @@ -204,7 +204,7 @@ TEST_F(SequentialWriterTest, sequantial_writer_call_metadata_update_on_bag_split std::string rmw_format = "rmw_format"; writer_->open(storage_options_, {rmw_format, rmw_format}); - writer_->create_topic({test_topic_name, test_topic_type, "", "", ""}); + writer_->create_topic({test_topic_name, test_topic_type, "", {}, ""}); auto message = std::make_shared(); message->topic_name = test_topic_name; @@ -283,7 +283,7 @@ TEST_F(SequentialWriterTest, bagfile_size_is_checked_on_every_write) { storage_options_.max_bagfile_size = max_bagfile_size; writer_->open(storage_options_, {rmw_format, rmw_format}); - writer_->create_topic({"test_topic", "test_msgs/BasicTypes", "", "", ""}); + writer_->create_topic({"test_topic", "test_msgs/BasicTypes", "", {}, ""}); for (auto i = 0; i < counter; ++i) { writer_->write(message); @@ -333,7 +333,7 @@ TEST_F(SequentialWriterTest, writer_splits_when_storage_bagfile_size_gt_max_bagf storage_options_.max_bagfile_size = max_bagfile_size; writer_->open(storage_options_, {rmw_format, rmw_format}); - writer_->create_topic({"test_topic", "test_msgs/BasicTypes", "", "", ""}); + writer_->create_topic({"test_topic", "test_msgs/BasicTypes", "", {}, ""}); for (auto i = 0; i < message_count; ++i) { writer_->write(message); @@ -411,7 +411,7 @@ TEST_F( storage_options_.snapshot_mode = false; writer_->open(storage_options_, {rmw_format, rmw_format}); - writer_->create_topic({"test_topic", "test_msgs/BasicTypes", "", "", ""}); + writer_->create_topic({"test_topic", "test_msgs/BasicTypes", "", {}, ""}); auto timeout = std::chrono::seconds(2); for (auto i = 1u; i < message_count; ++i) { @@ -491,7 +491,7 @@ TEST_F(SequentialWriterTest, do_not_use_cache_if_cache_size_is_zero) { storage_options_.max_cache_size = max_cache_size; writer_->open(storage_options_, {rmw_format, rmw_format}); - writer_->create_topic({"test_topic", "test_msgs/BasicTypes", "", "", ""}); + writer_->create_topic({"test_topic", "test_msgs/BasicTypes", "", {}, ""}); for (auto i = 0u; i < counter; ++i) { writer_->write(message); @@ -525,7 +525,7 @@ TEST_F(SequentialWriterTest, snapshot_mode_write_on_trigger) msg_content.c_str(), msg_length); writer_->open(storage_options_, {rmw_format, rmw_format}); - writer_->create_topic({"test_topic", "test_msgs/BasicTypes", "", "", ""}); + writer_->create_topic({"test_topic", "test_msgs/BasicTypes", "", {}, ""}); for (auto i = 0u; i < 100; ++i) { writer_->write(message); @@ -561,7 +561,7 @@ TEST_F(SequentialWriterTest, snapshot_mode_not_triggered_no_storage_write) msg_content.c_str(), msg_length); writer_->open(storage_options_, {rmw_format, rmw_format}); - writer_->create_topic({"test_topic", "test_msgs/BasicTypes", "", "", ""}); + writer_->create_topic({"test_topic", "test_msgs/BasicTypes", "", {}, ""}); for (auto i = 0u; i < 100; ++i) { writer_->write(message); @@ -630,7 +630,7 @@ TEST_F(SequentialWriterTest, split_event_calls_callback) writer_->add_event_callbacks(callbacks); writer_->open(storage_options_, {"rmw_format", "rmw_format"}); - writer_->create_topic({"test_topic", "test_msgs/BasicTypes", "", "", ""}); + writer_->create_topic({"test_topic", "test_msgs/BasicTypes", "", {}, ""}); for (auto i = 0; i < message_count; ++i) { writer_->write(message); @@ -689,7 +689,7 @@ TEST_F(SequentialWriterTest, split_event_calls_on_writer_close) writer_->add_event_callbacks(callbacks); writer_->open(storage_options_, {"rmw_format", "rmw_format"}); - writer_->create_topic({"test_topic", "test_msgs/BasicTypes", "", "", ""}); + writer_->create_topic({"test_topic", "test_msgs/BasicTypes", "", {}, ""}); for (auto i = 0; i < message_count; ++i) { writer_->write(message); diff --git a/rosbag2_cpp/test/rosbag2_cpp/test_storage_without_metadata_file.cpp b/rosbag2_cpp/test/rosbag2_cpp/test_storage_without_metadata_file.cpp index 8fddc3c1ba..e605403df8 100644 --- a/rosbag2_cpp/test/rosbag2_cpp/test_storage_without_metadata_file.cpp +++ b/rosbag2_cpp/test/rosbag2_cpp/test_storage_without_metadata_file.cpp @@ -56,7 +56,7 @@ TEST_F(StorageWithoutMetadataFileTest, open_uses_storage_id_from_storage_options "topic", "test_msgs/BasicTypes", kRmwFormat, - "", + {}, "" }; diff --git a/rosbag2_examples/rosbag2_examples_cpp/src/data_generator_executable.cpp b/rosbag2_examples/rosbag2_examples_cpp/src/data_generator_executable.cpp index 86a7ea9613..0e00134d2f 100644 --- a/rosbag2_examples/rosbag2_examples_cpp/src/data_generator_executable.cpp +++ b/rosbag2_examples/rosbag2_examples_cpp/src/data_generator_executable.cpp @@ -37,7 +37,7 @@ int main(int, char **) "synthetic", "example_interfaces/msg/Int32", rmw_get_serialization_format(), - "", + {}, "" }); diff --git a/rosbag2_examples/rosbag2_examples_cpp/src/data_generator_node.cpp b/rosbag2_examples/rosbag2_examples_cpp/src/data_generator_node.cpp index 585f860ea7..787995fc28 100644 --- a/rosbag2_examples/rosbag2_examples_cpp/src/data_generator_node.cpp +++ b/rosbag2_examples/rosbag2_examples_cpp/src/data_generator_node.cpp @@ -38,7 +38,7 @@ class DataGenerator : public rclcpp::Node "synthetic", "example_interfaces/msg/Int32", rmw_get_serialization_format(), - "", + {}, "", }); diff --git a/rosbag2_py/src/rosbag2_py/_storage.cpp b/rosbag2_py/src/rosbag2_py/_storage.cpp index 56a719bb38..8d9c7285ec 100644 --- a/rosbag2_py/src/rosbag2_py/_storage.cpp +++ b/rosbag2_py/src/rosbag2_py/_storage.cpp @@ -23,6 +23,7 @@ #include "rosbag2_storage/storage_interfaces/base_read_interface.hpp" #include "rosbag2_storage/storage_options.hpp" #include "rosbag2_storage/topic_metadata.hpp" +#include "rosbag2_storage/qos.hpp" #include "./format_bag_metadata.hpp" @@ -152,13 +153,69 @@ PYBIND11_MODULE(_storage, m) { &rosbag2_storage::MessageDefinition::encoded_message_definition) .def_readwrite("type_hash", &rosbag2_storage::MessageDefinition::type_hash); + pybind11::enum_(m, "rmw_qos_history_policy_t") + .value("RMW_QOS_POLICY_HISTORY_SYSTEM_DEFAULT", RMW_QOS_POLICY_HISTORY_SYSTEM_DEFAULT) + .value("RMW_QOS_POLICY_HISTORY_KEEP_LAST", RMW_QOS_POLICY_HISTORY_KEEP_LAST) + .value("RMW_QOS_POLICY_HISTORY_KEEP_ALL", RMW_QOS_POLICY_HISTORY_KEEP_ALL) + .value("RMW_QOS_POLICY_HISTORY_UNKNOWN", RMW_QOS_POLICY_HISTORY_UNKNOWN); + + pybind11::enum_(m, "rmw_qos_reliability_policy_t") + .value("RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT", RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT) + .value("RMW_QOS_POLICY_RELIABILITY_RELIABLE", RMW_QOS_POLICY_RELIABILITY_RELIABLE) + .value("RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT", RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT) + .value("RMW_QOS_POLICY_RELIABILITY_UNKNOWN", RMW_QOS_POLICY_RELIABILITY_UNKNOWN); + + pybind11::enum_(m, "rmw_qos_durability_policy_t") + .value("RMW_QOS_POLICY_DURABILITY_SYSTEM_DEFAULT", RMW_QOS_POLICY_DURABILITY_SYSTEM_DEFAULT) + .value("RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL", RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL) + .value("RMW_QOS_POLICY_DURABILITY_VOLATILE", RMW_QOS_POLICY_DURABILITY_VOLATILE) + .value("RMW_QOS_POLICY_DURABILITY_UNKNOWN", RMW_QOS_POLICY_DURABILITY_UNKNOWN); + + + pybind11::enum_(m, "rmw_qos_liveliness_policy_t") + .value("RMW_QOS_POLICY_LIVELINESS_SYSTEM_DEFAULT", RMW_QOS_POLICY_LIVELINESS_SYSTEM_DEFAULT) + .value("RMW_QOS_POLICY_LIVELINESS_AUTOMATIC", RMW_QOS_POLICY_LIVELINESS_AUTOMATIC) + .value("RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_TOPIC", RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_TOPIC) + .value("RMW_QOS_POLICY_LIVELINESS_UNKNOWN", RMW_QOS_POLICY_LIVELINESS_UNKNOWN); + + pybind11::class_(m, "Duration") + .def( + pybind11::init(), + pybind11::arg("seconds"), + pybind11::arg("nanoseconds")); + + pybind11::class_(m, "QoS") + .def( + pybind11::init(), + pybind11::arg("history_depth")) + .def("keep_last", &rclcpp::QoS::keep_last) + .def("keep_all", &rclcpp::QoS::keep_all) + .def("reliable", &rclcpp::QoS::reliable) + .def("best_effort", &rclcpp::QoS::best_effort) + .def("durability_volatile", &rclcpp::QoS::durability_volatile) + .def("transient_local", &rclcpp::QoS::transient_local) + .def("history", pybind11::overload_cast(&rclcpp::QoS::history)) + .def( + "reliability", + pybind11::overload_cast(&rclcpp::QoS::reliability)) + .def("durability", pybind11::overload_cast(&rclcpp::QoS::durability)) + .def("liveliness", pybind11::overload_cast(&rclcpp::QoS::liveliness)) + .def("deadline", pybind11::overload_cast(&rclcpp::QoS::deadline)) + .def("lifespan", pybind11::overload_cast(&rclcpp::QoS::lifespan)) + .def( + "liveliness_lease_duration", + pybind11::overload_cast(&rclcpp::QoS::liveliness_lease_duration)) + .def( + "avoid_ros_namespace_conventions", + pybind11::overload_cast(&rclcpp::QoS::avoid_ros_namespace_conventions)); + pybind11::class_(m, "TopicMetadata") .def( - pybind11::init(), + pybind11::init, std::string>(), pybind11::arg("name"), pybind11::arg("type"), pybind11::arg("serialization_format"), - pybind11::arg("offered_qos_profiles") = "", + pybind11::arg("offered_qos_profiles") = std::vector(), pybind11::arg("type_description_hash") = "") .def_readwrite("name", &rosbag2_storage::TopicMetadata::name) .def_readwrite("type", &rosbag2_storage::TopicMetadata::type) diff --git a/rosbag2_py/src/rosbag2_py/_transport.cpp b/rosbag2_py/src/rosbag2_py/_transport.cpp index 0237bf5cfa..fad7d870ec 100644 --- a/rosbag2_py/src/rosbag2_py/_transport.cpp +++ b/rosbag2_py/src/rosbag2_py/_transport.cpp @@ -295,7 +295,7 @@ void bag_rewrite( rosbag2_storage::StorageOptions storage_options{}; YAML::convert::decode(bag_node, storage_options); rosbag2_transport::RecordOptions record_options = bag_rewrite_default_record_options(); - YAML::convert::decode(bag_node, record_options); + record_options = bag_node.as(); output_options.push_back(std::make_pair(storage_options, record_options)); } rosbag2_transport::bag_rewrite(input_options, output_options); diff --git a/rosbag2_storage/CMakeLists.txt b/rosbag2_storage/CMakeLists.txt index cd4d7ab601..1fbd3e6dba 100644 --- a/rosbag2_storage/CMakeLists.txt +++ b/rosbag2_storage/CMakeLists.txt @@ -25,11 +25,14 @@ find_package(ament_cmake REQUIRED) find_package(pluginlib REQUIRED) find_package(rcpputils REQUIRED) find_package(rcutils REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rmw REQUIRED) find_package(yaml_cpp_vendor REQUIRED) add_library( ${PROJECT_NAME} SHARED + src/rosbag2_storage/qos.cpp src/rosbag2_storage/default_storage_id.cpp src/rosbag2_storage/metadata_io.cpp src/rosbag2_storage/ros_helper.cpp @@ -45,6 +48,8 @@ target_link_libraries(${PROJECT_NAME} pluginlib::pluginlib rcpputils::rcpputils rcutils::rcutils + rclcpp::rclcpp + rmw::rmw yaml-cpp ) @@ -70,7 +75,7 @@ ament_export_libraries(${PROJECT_NAME}) # Export modern CMake targets ament_export_targets(export_${PROJECT_NAME}) -ament_export_dependencies(pluginlib yaml_cpp_vendor) +ament_export_dependencies(pluginlib yaml_cpp_vendor rclcpp rmw) if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) @@ -119,6 +124,14 @@ if(BUILD_TESTING) ${PROJECT_NAME} rosbag2_test_common::rosbag2_test_common ) -endif() + + ament_add_gmock(test_qos + test/rosbag2_storage/test_qos.cpp) + target_link_libraries(test_qos + ${PROJECT_NAME} + rosbag2_test_common::rosbag2_test_common + yaml-cpp + ) + endif() ament_package() diff --git a/rosbag2_storage/include/rosbag2_storage/bag_metadata.hpp b/rosbag2_storage/include/rosbag2_storage/bag_metadata.hpp index 553aa67f06..4bbc1cda99 100644 --- a/rosbag2_storage/include/rosbag2_storage/bag_metadata.hpp +++ b/rosbag2_storage/include/rosbag2_storage/bag_metadata.hpp @@ -42,7 +42,7 @@ struct FileInformation struct BagMetadata { - int version = 8; // upgrade this number when changing the content of the struct + int version = 9; // upgrade this number when changing the content of the struct uint64_t bag_size = 0; // Will not be serialized std::string storage_identifier; std::vector relative_file_paths; diff --git a/rosbag2_storage/include/rosbag2_storage/qos.hpp b/rosbag2_storage/include/rosbag2_storage/qos.hpp new file mode 100644 index 0000000000..baba001ce0 --- /dev/null +++ b/rosbag2_storage/include/rosbag2_storage/qos.hpp @@ -0,0 +1,190 @@ +// 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 ROSBAG2_STORAGE__QOS_HPP_ +#define ROSBAG2_STORAGE__QOS_HPP_ + +#include +#include +#include + +#include "rclcpp/node_interfaces/node_graph_interface.hpp" +#include "rclcpp/qos.hpp" + +#ifdef _WIN32 +// This is necessary because yaml-cpp does not always use dllimport/dllexport consistently +# pragma warning(push) +# pragma warning(disable:4251) +# pragma warning(disable:4275) +#endif +#include "yaml-cpp/yaml.h" +#ifdef _WIN32 +# pragma warning(pop) +#endif + +#include "rosbag2_storage/visibility_control.hpp" + +namespace rosbag2_storage +{ +/// Simple wrapper around rclcpp::QoS to provide a default constructor for YAML deserialization. +class ROSBAG2_STORAGE_PUBLIC Rosbag2QoS : public rclcpp::QoS +{ +public: + Rosbag2QoS() + : rclcpp::QoS(rmw_qos_profile_default.depth) {} + + explicit Rosbag2QoS(const rclcpp::QoS & value) + : rclcpp::QoS(value) {} + + Rosbag2QoS & default_history() + { + keep_last(rmw_qos_profile_default.depth); + return *this; + } + + // Create an adaptive QoS profile to use for subscribing to a set of offers from publishers. + /** + * - Uses rosbag2_storage defaults for History since they do not affect compatibility. + * - Adapts Durability and Reliability, falling back to the least strict publisher when there + * is a mixed offer. This behavior errs on the side of forming connections with all publishers. + * - Does not specify Lifespan, Deadline, or Liveliness to be maximally compatible, because + * these policies do not affect message delivery. + */ + static Rosbag2QoS adapt_request_to_offers( + const std::string & topic_name, + const std::vector & endpoints); + + // Create a QoS profile to offer for playback. + /** + * This logic exists because rosbag2 does not record on a per-publisher basis, so we try to + * get as close as possible to the original system's behavior, given a single publisher. + * If all profiles are the same (excepting History & Lifespan, which are purely local), + * that exact value is returned. + * Otherwise, fall back to the rosbag2 default and emit a warning. + */ + static Rosbag2QoS adapt_offer_to_recorded_offers( + const std::string & topic_name, + const std::vector & profiles); +}; + +ROSBAG2_STORAGE_PUBLIC std::vector from_rclcpp_qos_vector( + const std::vector & in); +ROSBAG2_STORAGE_PUBLIC std::string serialize_rclcpp_qos_vector( + const std::vector & in, + int version = 9); +ROSBAG2_STORAGE_PUBLIC std::vector to_rclcpp_qos_vector( + const std::string & serialized, + int version); +} // namespace rosbag2_storage + +namespace YAML +{ + +/// Pass metadata version to the sub-structs of BagMetadata for deserializing. +/** + * Encoding should always use the current metadata version, so it does not need this value. + * We cannot extend the YAML::Node class to include this, so we must call it + * as a function with the node as an argument. + */ + +template +T decode_for_version(const Node & node, int version) +{ + static_assert( + std::is_default_constructible::value, + "Type passed to decode_for_version that has is not default constructible."); + if (!node.IsDefined()) { + throw TypedBadConversion(node.Mark()); + } + T value{}; + if (convert::decode(node, value, version)) { + return value; + } + throw TypedBadConversion(node.Mark()); +} + +template<> +struct ROSBAG2_STORAGE_PUBLIC convert +{ + static Node encode(const rmw_qos_history_policy_t & policy, int version); + static bool decode(const Node & node, rmw_qos_history_policy_t & policy); +}; + +template<> +struct ROSBAG2_STORAGE_PUBLIC convert +{ + static Node encode(const rmw_qos_reliability_policy_t & policy, int version); + static bool decode(const Node & node, rmw_qos_reliability_policy_t & policy); +}; + +template<> +struct ROSBAG2_STORAGE_PUBLIC convert +{ + static Node encode(const rmw_qos_durability_policy_t & policy, int version); + static bool decode(const Node & node, rmw_qos_durability_policy_t & policy); +}; + +template<> +struct ROSBAG2_STORAGE_PUBLIC convert +{ + static Node encode(const rmw_qos_liveliness_policy_t & policy, int version); + static bool decode(const Node & node, rmw_qos_liveliness_policy_t & policy); +}; + +template<> +struct ROSBAG2_STORAGE_PUBLIC convert +{ + static Node encode(const rmw_time_t & time); + static bool decode(const Node & node, rmw_time_t & time); +}; + +template<> +struct ROSBAG2_STORAGE_PUBLIC convert +{ + static Node encode(const rclcpp::QoS & qos, int version = 9); + static bool decode(const Node & node, rclcpp::QoS & qos, int version = 9); +}; + +template<> +struct ROSBAG2_STORAGE_PUBLIC convert +{ + static Node encode(const rosbag2_storage::Rosbag2QoS & qos, int version = 9); + static bool decode(const Node & node, rosbag2_storage::Rosbag2QoS & qos, int version = 9); +}; + +template<> +struct ROSBAG2_STORAGE_PUBLIC convert> +{ + static Node encode(const std::vector & rhs); + static bool decode( + const Node & node, std::vector & rhs, int version = 9); +}; + +template<> +struct ROSBAG2_STORAGE_PUBLIC convert> +{ + static Node encode(const std::vector & rhs, int version = 9); + static bool decode(const Node & node, std::vector & rhs, int version = 9); +}; + +template<> +struct ROSBAG2_STORAGE_PUBLIC convert> +{ + static Node encode(const std::map & rhs); + static bool decode( + const Node & node, std::map & rhs, int version = 9); +}; +} // namespace YAML + +#endif // ROSBAG2_STORAGE__QOS_HPP_ diff --git a/rosbag2_storage/include/rosbag2_storage/topic_metadata.hpp b/rosbag2_storage/include/rosbag2_storage/topic_metadata.hpp index cb8339f238..fec92b6127 100644 --- a/rosbag2_storage/include/rosbag2_storage/topic_metadata.hpp +++ b/rosbag2_storage/include/rosbag2_storage/topic_metadata.hpp @@ -16,6 +16,8 @@ #define ROSBAG2_STORAGE__TOPIC_METADATA_HPP_ #include +#include +#include "rclcpp/qos.hpp" namespace rosbag2_storage { @@ -25,8 +27,7 @@ struct TopicMetadata std::string name; std::string type; std::string serialization_format; - // Serialized std::vector as a YAML string - std::string offered_qos_profiles; + std::vector offered_qos_profiles; // REP-2011 type description hash if available for topic, "" otherwise. std::string type_description_hash; diff --git a/rosbag2_storage/include/rosbag2_storage/yaml.hpp b/rosbag2_storage/include/rosbag2_storage/yaml.hpp index 601c9a2dbe..a71a830cc4 100644 --- a/rosbag2_storage/include/rosbag2_storage/yaml.hpp +++ b/rosbag2_storage/include/rosbag2_storage/yaml.hpp @@ -14,6 +14,7 @@ #ifndef ROSBAG2_STORAGE__YAML_HPP_ #define ROSBAG2_STORAGE__YAML_HPP_ +#include #include #include #include @@ -30,6 +31,7 @@ #endif #include "rosbag2_storage/bag_metadata.hpp" +#include "rosbag2_storage/qos.hpp" namespace YAML { @@ -64,38 +66,22 @@ struct convert> } }; -/// Pass metadata version to the sub-structs of BagMetadata for deserializing. -/** - * Encoding should always use the current metadata version, so it does not need this value. - * We cannot extend the YAML::Node class to include this, so we must call it - * as a function with the node as an argument. - */ -template -T decode_for_version(const Node & node, int version) -{ - static_assert( - std::is_default_constructible::value, - "Type passed to decode_for_version that has is not default constructible."); - if (!node.IsDefined()) { - throw TypedBadConversion(node.Mark()); - } - T value{}; - if (convert::decode(node, value, version)) { - return value; - } - throw TypedBadConversion(node.Mark()); -} - template<> struct convert { - static Node encode(const rosbag2_storage::TopicMetadata & topic) + static Node encode(const rosbag2_storage::TopicMetadata & topic, int version) { Node node; node["name"] = topic.name; node["type"] = topic.type; node["serialization_format"] = topic.serialization_format; - node["offered_qos_profiles"] = topic.offered_qos_profiles; + if (version < 9) { + node["offered_qos_profiles"] = rosbag2_storage::serialize_rclcpp_qos_vector( + topic.offered_qos_profiles, version); + } else { + node["offered_qos_profiles"] = YAML::convert>::encode( + topic.offered_qos_profiles, version); + } node["type_description_hash"] = topic.type_description_hash; return node; } @@ -105,10 +91,12 @@ struct convert topic.name = node["name"].as(); topic.type = node["type"].as(); topic.serialization_format = node["serialization_format"].as(); - if (version >= 4) { - topic.offered_qos_profiles = node["offered_qos_profiles"].as(); - } else { - topic.offered_qos_profiles = ""; + if (version >= 9) { + topic.offered_qos_profiles = + YAML::decode_for_version>(node["offered_qos_profiles"], version); + } else if (version >= 4) { + std::string qos_str = node["offered_qos_profiles"].as(); + topic.offered_qos_profiles = rosbag2_storage::to_rclcpp_qos_vector(qos_str, version); } if (version >= 7) { topic.type_description_hash = node["type_description_hash"].as(); @@ -122,11 +110,12 @@ struct convert template<> struct convert { - static Node encode(const rosbag2_storage::TopicInformation & metadata) + static Node encode(const rosbag2_storage::TopicInformation & topic_info, int version) { Node node; - node["topic_metadata"] = metadata.topic_metadata; - node["message_count"] = metadata.message_count; + node["topic_metadata"] = + convert::encode(topic_info.topic_metadata, version); + node["message_count"] = topic_info.message_count; return node; } @@ -142,11 +131,11 @@ struct convert template<> struct convert> { - static Node encode(const std::vector & rhs) + static Node encode(const std::vector & rhs, int version) { Node node{NodeType::Sequence}; for (const auto & value : rhs) { - node.push_back(value); + node.push_back(convert::encode(value, version)); } return node; } @@ -237,7 +226,9 @@ struct convert node["duration"] = metadata.duration; node["starting_time"] = metadata.starting_time; node["message_count"] = metadata.message_count; - node["topics_with_message_count"] = metadata.topics_with_message_count; + node["topics_with_message_count"] = + convert>::encode( + metadata.topics_with_message_count, metadata.version); node["compression_format"] = metadata.compression_format; node["compression_mode"] = metadata.compression_mode; node["relative_file_paths"] = metadata.relative_file_paths; diff --git a/rosbag2_storage/package.xml b/rosbag2_storage/package.xml index d3086822a8..3d5889074d 100644 --- a/rosbag2_storage/package.xml +++ b/rosbag2_storage/package.xml @@ -15,6 +15,8 @@ pluginlib rcpputils rcutils + rclcpp + rmw yaml_cpp_vendor ament_cmake_gtest diff --git a/rosbag2_storage/src/rosbag2_storage/qos.cpp b/rosbag2_storage/src/rosbag2_storage/qos.cpp new file mode 100644 index 0000000000..8c3e165d68 --- /dev/null +++ b/rosbag2_storage/src/rosbag2_storage/qos.cpp @@ -0,0 +1,451 @@ +// 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 "rosbag2_storage/qos.hpp" +#include "rosbag2_storage/logging.hpp" +#include "rmw/qos_string_conversions.h" + +namespace +{ +/** + * The following constants were the "Inifinity" value returned by RMW implementations before + * the introduction of RMW_DURATION_INFINITE and associated RMW fixes + * RMW: https://github.com/ros2/rmw/pull/301 + * Fast-DDS: https://github.com/ros2/rmw_fastrtps/pull/515 + * CycloneDDS: https://github.com/ros2/rmw_cyclonedds/pull/288 + * RTI Connext: https://github.com/ros2/rmw_connext/pull/491 + * + * These values exist in bags recorded in Foxy, they need to be translated to RMW_DURATION_INFINITE + * to be consistently understood for playback. + * With those values, if a bag is played back in a different implementation than it was recorded, + * the publishers will fail to be created with an error indicating an invalid QoS value.. + */ +static const rmw_time_t RMW_CYCLONEDDS_FOXY_INFINITE = rmw_time_from_nsec(0x7FFFFFFFFFFFFFFFll); +static const rmw_time_t RMW_FASTRTPS_FOXY_INFINITE {0x7FFFFFFFll, 0xFFFFFFFFll}; +static const rmw_time_t RMW_CONNEXT_FOXY_INFINITE {0x7FFFFFFFll, 0x7FFFFFFFll}; +} // namespace + +namespace YAML +{ + +Node convert::encode(const rmw_qos_history_policy_t & policy, int version) +{ + if (version < 9) { + return Node(static_cast(policy)); + } + if (policy == RMW_QOS_POLICY_HISTORY_UNKNOWN) { + return Node(std::string("unknown")); + } else { + return Node(std::string(rmw_qos_history_policy_to_str(policy))); + } +} + +bool convert::decode(const Node & node, rmw_qos_history_policy_t & policy) +{ + policy = rmw_qos_history_policy_from_str(node.as().c_str()); + return true; +} + +Node convert::encode( + const rmw_qos_reliability_policy_t & policy, + int version) +{ + if (version < 9) { + return Node(static_cast(policy)); + } + if (policy == RMW_QOS_POLICY_RELIABILITY_UNKNOWN) { + return Node(std::string("unknown")); + } else { + return Node(std::string(rmw_qos_reliability_policy_to_str(policy))); + } +} + +bool convert::decode( + const Node & node, + rmw_qos_reliability_policy_t & policy) +{ + policy = rmw_qos_reliability_policy_from_str(node.as().c_str()); + return true; +} + +Node convert::encode( + const rmw_qos_durability_policy_t & policy, + int version) +{ + if (version < 9) { + return Node(static_cast(policy)); + } + if (policy == RMW_QOS_POLICY_DURABILITY_UNKNOWN) { + return Node(std::string("unknown")); + } else { + return Node(std::string(rmw_qos_durability_policy_to_str(policy))); + } +} + +bool convert::decode( + const Node & node, + rmw_qos_durability_policy_t & policy) +{ + policy = rmw_qos_durability_policy_from_str(node.as().c_str()); + return true; +} + +Node convert::encode( + const rmw_qos_liveliness_policy_t & policy, + int version) +{ + if (version < 9) { + return Node(static_cast(policy)); + } + if (policy == RMW_QOS_POLICY_LIVELINESS_UNKNOWN) { + return Node(std::string("unknown")); + } else { + return Node(std::string(rmw_qos_liveliness_policy_to_str(policy))); + } +} + +bool convert::decode( + const Node & node, + rmw_qos_liveliness_policy_t & policy) +{ + policy = rmw_qos_liveliness_policy_from_str(node.as().c_str()); + return true; +} + +Node convert::encode(const rmw_time_t & time) +{ + Node node; + node["sec"] = time.sec; + node["nsec"] = time.nsec; + return node; +} + +bool convert::decode(const Node & node, rmw_time_t & time) +{ + time.sec = node["sec"].as(); + time.nsec = node["nsec"].as(); + if ( + rmw_time_equal(time, RMW_CYCLONEDDS_FOXY_INFINITE) || + rmw_time_equal(time, RMW_FASTRTPS_FOXY_INFINITE) || + rmw_time_equal(time, RMW_CONNEXT_FOXY_INFINITE)) + { + time = RMW_DURATION_INFINITE; + } + return true; +} + +Node convert::encode(const rclcpp::QoS & qos, int version) +{ + const auto & p = qos.get_rmw_qos_profile(); + Node node; + node["history"] = convert::encode(p.history, version); + node["depth"] = p.depth; + node["reliability"] = convert::encode(p.reliability, version); + node["durability"] = convert::encode(p.durability, version); + node["deadline"] = p.deadline; + node["lifespan"] = p.lifespan; + node["liveliness"] = convert::encode(p.liveliness, version); + node["liveliness_lease_duration"] = p.liveliness_lease_duration; + node["avoid_ros_namespace_conventions"] = p.avoid_ros_namespace_conventions; + return node; +} + +bool convert::decode(const Node & node, rclcpp::QoS & qos, int version) +{ + rmw_qos_history_policy_t history; + rmw_qos_reliability_policy_t reliability; + rmw_qos_durability_policy_t durability; + rmw_qos_liveliness_policy_t liveliness; + + // Try to auto-detect qos serialization format + int history_int = -1; + if (convert::decode(node["history"], history_int) && + history_int >= RMW_QOS_POLICY_HISTORY_SYSTEM_DEFAULT && + history_int <= RMW_QOS_POLICY_HISTORY_UNKNOWN) + { + history = static_cast(history_int); + version = 8; + } else { + history = node["history"].as(); + version = 9; + } + + if (version <= 8) { + reliability = static_cast(node["reliability"].as()); + durability = static_cast(node["durability"].as()); + liveliness = static_cast(node["liveliness"].as()); + } else { + reliability = node["reliability"].as(); + durability = node["durability"].as(); + liveliness = node["liveliness"].as(); + } + + if (history == RMW_QOS_POLICY_HISTORY_KEEP_LAST) { + qos.keep_last(node["depth"].as()); + } else { + qos.history(history); + } + + qos + .reliability(reliability) + .durability(durability) + .deadline(node["deadline"].as()) + .lifespan(node["lifespan"].as()) + .liveliness(liveliness) + .liveliness_lease_duration(node["liveliness_lease_duration"].as()) + .avoid_ros_namespace_conventions(node["avoid_ros_namespace_conventions"].as()); + + return true; +} + +Node convert::encode( + const rosbag2_storage::Rosbag2QoS & qos, int version) +{ + return convert::encode(static_cast(qos), version); +} + +bool convert::decode( + const Node & node, rosbag2_storage::Rosbag2QoS & qos, int version) +{ + return convert::decode(node, qos, version); +} + +Node convert>::encode( + const std::vector & rhs) +{ + Node node{NodeType::Sequence}; + for (const auto & value : rhs) { + node.push_back(value); + } + return node; +} + +bool convert>::decode( + const Node & node, std::vector & rhs, int version) +{ + if (!node.IsSequence()) { + return false; + } + + rhs.clear(); + for (const auto & value : node) { + rhs.push_back(decode_for_version(value, version)); + } + return true; +} + +Node convert>::encode(const std::vector & rhs, int version) +{ + Node node{NodeType::Sequence}; + for (const auto & value : rhs) { + node.push_back(convert::encode(value, version)); + } + return node; +} + +bool convert>::decode( + const Node & node, std::vector & rhs, int version) +{ + if (!node.IsSequence()) { + return false; + } + + rhs.clear(); + for (const auto & value : node) { + // Using rosbag2_storage::Rosbag2QoS for decoding because rclcpp::QoS is not default + // constructable. Note: It is safe to use upcast when adding to the vector + auto rosbag2_qos = decode_for_version(value, version); + rhs.push_back(rosbag2_qos); + } + return true; +} + +Node convert>::encode( + const std::map & rhs) +{ + Node node{NodeType::Sequence}; + for (const auto & [key, value] : rhs) { + node.force_insert(key, value); + } + return node; +} + +bool convert>::decode( + const Node & node, std::map & rhs, int version) +{ + if (!node.IsMap()) { + return false; + } + + rhs.clear(); + for (const auto & element : node) { + rhs[element.first.as()] = decode_for_version( + element.second, version); + } + return true; +} +} // namespace YAML + +namespace rosbag2_storage +{ +Rosbag2QoS Rosbag2QoS::adapt_request_to_offers( + const std::string & topic_name, const std::vector & endpoints) +{ + if (endpoints.empty()) { + return Rosbag2QoS{}; + } + size_t num_endpoints = endpoints.size(); + size_t reliability_reliable_endpoints_count = 0; + size_t durability_transient_local_endpoints_count = 0; + for (const auto & endpoint : endpoints) { + const auto & profile = endpoint.qos_profile().get_rmw_qos_profile(); + if (profile.reliability == RMW_QOS_POLICY_RELIABILITY_RELIABLE) { + reliability_reliable_endpoints_count++; + } + if (profile.durability == RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL) { + durability_transient_local_endpoints_count++; + } + } + + // We set policies in order as defined in rmw_qos_profile_t + Rosbag2QoS request_qos{}; + // Policy: history, depth + // History does not affect compatibility + + // Policy: reliability + if (reliability_reliable_endpoints_count == num_endpoints) { + request_qos.reliable(); + } else { + if (reliability_reliable_endpoints_count > 0) { + ROSBAG2_STORAGE_LOG_WARN_STREAM( + "Some, but not all, publishers on topic \"" << topic_name << "\" " + "are offering RMW_QOS_POLICY_RELIABILITY_RELIABLE. " + "Falling back to RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT " + "as it will connect to all publishers. " + "Some messages from Reliable publishers could be dropped."); + } + request_qos.best_effort(); + } + + // Policy: durability + // If all publishers offer transient_local, we can request it and receive latched messages + if (durability_transient_local_endpoints_count == num_endpoints) { + request_qos.transient_local(); + } else { + if (durability_transient_local_endpoints_count > 0) { + ROSBAG2_STORAGE_LOG_WARN_STREAM( + "Some, but not all, publishers on topic \"" << topic_name << "\" " + "are offering RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL. " + "Falling back to RMW_QOS_POLICY_DURABILITY_VOLATILE " + "as it will connect to all publishers. " + "Previously-published latched messages will not be retrieved."); + } + request_qos.durability_volatile(); + } + // Policy: deadline + // Deadline does not affect delivery of messages, + // and we do not record Deadline"Missed events. + // We can always use unspecified deadline, which will be compatible with all publishers. + + // Policy: lifespan + // Lifespan does not affect compatibiliy + + // Policy: liveliness, liveliness_lease_duration + // Liveliness does not affect delivery of messages, + // and we do not record LivelinessChanged events. + // We can always use unspecified liveliness, which will be compatible with all publishers. + return request_qos; +} + +namespace +{ +bool operator==(const rmw_time_t & lhs, const rmw_time_t & rhs) +{ + return lhs.sec == rhs.sec && lhs.nsec == rhs.nsec; +} + +/** Check if all QoS profiles in the vector are identical when only looking at + * policies that affect compatibility. + * This means it excludes history and lifespan from the equality check. + */ +bool all_profiles_effectively_same(const std::vector & profiles) +{ + auto iterator = profiles.begin(); + const auto p_ref = iterator->get_rmw_qos_profile(); + iterator++; + for (; iterator != profiles.end(); iterator++) { + const auto p_next = iterator->get_rmw_qos_profile(); + bool compatibility_equals_previous = ( + // excluding history + p_ref.reliability == p_next.reliability && + p_ref.durability == p_next.durability && + p_ref.deadline == p_next.deadline && + // excluding lifespan + p_ref.liveliness == p_next.liveliness && + p_ref.liveliness_lease_duration == p_next.liveliness_lease_duration + ); + if (!compatibility_equals_previous) { + return false; + } + } + return true; +} +} // unnamed namespace + +Rosbag2QoS Rosbag2QoS::adapt_offer_to_recorded_offers( + const std::string & topic_name, const std::vector & profiles) +{ + if (profiles.empty()) { + return Rosbag2QoS{}; + } + if (all_profiles_effectively_same(profiles)) { + auto result = profiles[0]; + return result.default_history(); + } + + ROSBAG2_STORAGE_LOG_WARN_STREAM( + "Not all original publishers on topic " << topic_name << " offered the same QoS profiles. " + "Rosbag2 cannot yet choose an adapted profile to offer for this mixed case. " + "Falling back to the rosbag2_storage default publisher offer."); + return Rosbag2QoS{}; +} + +std::vector from_rclcpp_qos_vector(const std::vector & in) +{ + std::vector out; + out.reserve(in.size()); + std::transform( + in.begin(), in.end(), std::back_inserter(out), + [](auto & qos) {return static_cast(qos);}); + return out; +} + +std::string serialize_rclcpp_qos_vector(const std::vector & in, int version) +{ + auto node = YAML::convert>::encode(in, version); + return YAML::Dump(node); +} + +std::vector to_rclcpp_qos_vector(const std::string & serialized, int version) +{ + if (serialized.empty()) {return {};} + auto node = YAML::Load(serialized); + return YAML::decode_for_version>(node, version); +} + +} // namespace rosbag2_storage diff --git a/rosbag2_storage/test/rosbag2_storage/test_metadata_serialization.cpp b/rosbag2_storage/test/rosbag2_storage/test_metadata_serialization.cpp index 084aa0d2c1..277f6e18b6 100644 --- a/rosbag2_storage/test/rosbag2_storage/test_metadata_serialization.cpp +++ b/rosbag2_storage/test/rosbag2_storage/test_metadata_serialization.cpp @@ -54,8 +54,8 @@ TEST_F(MetadataFixture, test_writing_and_reading_yaml) metadata.starting_time = std::chrono::time_point(std::chrono::nanoseconds(1000000)); metadata.message_count = 50; - metadata.topics_with_message_count.push_back({{"topic1", "type1", "rmw1", "qos1", ""}, 100}); - metadata.topics_with_message_count.push_back({{"topic2", "type2", "rmw2", "qos2", ""}, 200}); + metadata.topics_with_message_count.push_back({{"topic1", "type1", "rmw1", {}, ""}, 100}); + metadata.topics_with_message_count.push_back({{"topic2", "type2", "rmw2", {}, ""}, 200}); metadata_io_->write_metadata(temporary_dir_path_, metadata); auto read_metadata = metadata_io_->read_metadata(temporary_dir_path_); @@ -99,7 +99,8 @@ TEST_F(MetadataFixture, metadata_reads_v3_check_offered_qos_profiles_empty) { // Make sure that when reading a v3 metadata, the deserialization // does not attempt to fill the offered_qos_profiles field - const std::string offered_qos_profiles = "qos_profile_string_data"; + std::vector offered_qos_profiles; + offered_qos_profiles.push_back(rclcpp::QoS(1)); const size_t message_count = 100; // arbitrary value BagMetadata metadata{}; @@ -118,7 +119,8 @@ TEST_F(MetadataFixture, metadata_reads_v3_check_offered_qos_profiles_empty) TEST_F(MetadataFixture, metadata_reads_v4_fills_offered_qos_profiles) { // Make sure when reading a v4 metadata that the deserialization fills "offered_qos_profiles" - const std::string offered_qos_profiles = "qos_profile_string_data"; + std::vector offered_qos_profiles; + offered_qos_profiles.push_back(rclcpp::QoS(1)); const size_t message_count = 100; // arbitrary value BagMetadata metadata{}; @@ -154,7 +156,7 @@ TEST_F(MetadataFixture, metadata_reads_v7_topic_type_hash) BagMetadata metadata{}; metadata.version = 7; metadata.topics_with_message_count.push_back( - {{"topic", "type", "rmw", "", type_description_hash}, 1}); + {{"topic", "type", "rmw", {rclcpp::QoS(1)}, type_description_hash}, 1}); metadata_io_->write_metadata(temporary_dir_path_, metadata); auto read_metadata = metadata_io_->read_metadata(temporary_dir_path_); diff --git a/rosbag2_transport/test/rosbag2_transport/test_qos.cpp b/rosbag2_storage/test/rosbag2_storage/test_qos.cpp similarity index 92% rename from rosbag2_transport/test/rosbag2_transport/test_qos.cpp rename to rosbag2_storage/test/rosbag2_storage/test_qos.cpp index 3c715d6cb3..39c4fa2cef 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_qos.cpp +++ b/rosbag2_storage/test/rosbag2_storage/test_qos.cpp @@ -19,20 +19,20 @@ #include "rmw/types.h" -#include "rosbag2_transport/qos.hpp" +#include "rosbag2_storage/qos.hpp" TEST(TestQoS, serialization) { - rosbag2_transport::Rosbag2QoS expected_qos; + rosbag2_storage::Rosbag2QoS expected_qos; YAML::Node offered_qos_profiles; offered_qos_profiles.push_back(expected_qos); std::string serialized = YAML::Dump(offered_qos_profiles); YAML::Node loaded_node = YAML::Load(serialized); - auto deserialized_profiles = loaded_node.as>(); + auto deserialized_profiles = loaded_node.as>(); ASSERT_EQ(deserialized_profiles.size(), 1u); - rosbag2_transport::Rosbag2QoS actual_qos = deserialized_profiles[0]; + rosbag2_storage::Rosbag2QoS actual_qos = deserialized_profiles[0]; EXPECT_EQ(actual_qos, expected_qos); } @@ -58,13 +58,15 @@ TEST(TestQoS, supports_version_4) " avoid_ros_namespace_conventions: false\n"; YAML::Node loaded_node = YAML::Load(serialized_profiles); - auto deserialized_profiles = loaded_node.as>(); + // Intentionally use loaded_node.as<..> to make sure that old format will be automatically + // detected and properly decoded by yaml parser. + auto deserialized_profiles = loaded_node.as>(); ASSERT_EQ(deserialized_profiles.size(), 1u); auto actual_qos = deserialized_profiles[0].get_rmw_qos_profile(); rmw_time_t zerotime{0, 0}; // Explicitly set up the same QoS profile in case defaults change - auto expected_qos = rosbag2_transport::Rosbag2QoS{} + auto expected_qos = rosbag2_storage::Rosbag2QoS{} .default_history() .reliable() .durability_volatile() @@ -105,7 +107,7 @@ TEST(TestQoS, translates_bad_infinity_values) {0x7FFFFFFFll, 0x7FFFFFFFll} // connext }; rmw_time_t infinity = RMW_DURATION_INFINITE; - const auto expected_qos = rosbag2_transport::Rosbag2QoS{} + const auto expected_qos = rosbag2_storage::Rosbag2QoS{} .default_history() .reliable() .durability_volatile() @@ -118,23 +120,23 @@ TEST(TestQoS, translates_bad_infinity_values) for (const auto & infinity : bad_infinities) { std::ostringstream serialized_profile; serialized_profile << - "history: 1\n" + "history: system_default\n" "depth: 10\n" - "reliability: 1\n" - "durability: 2\n" + "reliability: reliable\n" + "durability: volatile\n" "deadline:\n" " sec: " << infinity.sec << "\n" " nsec: " << infinity.nsec << "\n" "lifespan:\n" " sec: " << infinity.sec << "\n" " nsec: " << infinity.nsec << "\n" - "liveliness: 0\n" + "liveliness: system_default\n" "liveliness_lease_duration:\n" " sec: " << infinity.sec << "\n" " nsec: " << infinity.nsec << "\n" "avoid_ros_namespace_conventions: false\n"; const YAML::Node loaded_node = YAML::Load(serialized_profile.str()); - const auto deserialized_profile = loaded_node.as(); + const auto deserialized_profile = loaded_node.as(); const auto actual_qos = deserialized_profile.get_rmw_qos_profile(); EXPECT_TRUE(rmw_time_equal(actual_qos.lifespan, expected_qos.lifespan)); EXPECT_TRUE(rmw_time_equal(actual_qos.deadline, expected_qos.deadline)); @@ -144,10 +146,10 @@ TEST(TestQoS, translates_bad_infinity_values) } } -using rosbag2_transport::Rosbag2QoS; // NOLINT class AdaptiveQoSTest : public ::testing::Test { public: + using Rosbag2QoS = rosbag2_storage::Rosbag2QoS; AdaptiveQoSTest() = default; rclcpp::TopicEndpointInfo make_endpoint(const rclcpp::QoS & qos) @@ -170,8 +172,8 @@ class AdaptiveQoSTest : public ::testing::Test const std::string topic_name_{"/topic"}; std::vector endpoints_{}; - const rosbag2_transport::Rosbag2QoS default_offer_{}; - const rosbag2_transport::Rosbag2QoS default_request_{}; + const Rosbag2QoS default_offer_{}; + const Rosbag2QoS default_request_{}; }; TEST_F(AdaptiveQoSTest, adapt_request_empty_returns_default) diff --git a/rosbag2_storage_mcap/src/mcap_storage.cpp b/rosbag2_storage_mcap/src/mcap_storage.cpp index bcc0269671..20a6633abe 100644 --- a/rosbag2_storage_mcap/src/mcap_storage.cpp +++ b/rosbag2_storage_mcap/src/mcap_storage.cpp @@ -400,6 +400,10 @@ void MCAPStorage::read_metadata() if (!serialized_metadata.empty()) { YAML::Node metadata_node = YAML::Load(serialized_metadata); YAML::convert::decode(metadata_node, metadata_); + } else { + metadata_.version = 8; // Workaround to properly convert topic_metadata.offered_qos_profiles + // for old metadata versions. Assuming that if serialized metadata is not present then + // metadata_.version < 9 } try { metadata_.ros_distro = mcap_metadata.metadata.at("ROS_DISTRO"); @@ -438,7 +442,8 @@ void MCAPStorage::read_metadata() // Look up the offered_qos_profiles metadata entry const auto metadata_it = channel.metadata.find("offered_qos_profiles"); if (metadata_it != channel.metadata.end()) { - topic_info.topic_metadata.offered_qos_profiles = metadata_it->second; + topic_info.topic_metadata.offered_qos_profiles = + rosbag2_storage::to_rclcpp_qos_vector(metadata_it->second, metadata_.version); } const auto type_hash_it = channel.metadata.find("topic_type_hash"); if (type_hash_it != channel.metadata.end()) { @@ -815,8 +820,9 @@ void MCAPStorage::create_topic(const rosbag2_storage::TopicMetadata & topic, channel.topic = topic.name; channel.messageEncoding = topic_info.topic_metadata.serialization_format; channel.schemaId = schema_id; - channel.metadata.emplace("offered_qos_profiles", - topic_info.topic_metadata.offered_qos_profiles); + channel.metadata.emplace( + "offered_qos_profiles", + rosbag2_storage::serialize_rclcpp_qos_vector(topic_info.topic_metadata.offered_qos_profiles)); channel.metadata.emplace("topic_type_hash", topic_info.topic_metadata.type_description_hash); mcap_writer_->addChannel(channel); channel_ids_.emplace(topic.name, channel.id); diff --git a/rosbag2_storage_mcap/test/rosbag2_storage_mcap/test_mcap_storage.cpp b/rosbag2_storage_mcap/test/rosbag2_storage_mcap/test_mcap_storage.cpp index 28fe66ae92..4daba43f2b 100644 --- a/rosbag2_storage_mcap/test/rosbag2_storage_mcap/test_mcap_storage.cpp +++ b/rosbag2_storage_mcap/test/rosbag2_storage_mcap/test_mcap_storage.cpp @@ -96,10 +96,10 @@ TEST_F(McapStorageTestFixture, can_store_and_read_metadata_correctly) std::vector string_messages = {"first message", "second message", "third message"}; std::vector topics = {"topic1", "topic2"}; - rosbag2_storage::TopicMetadata topic_metadata_1 = {topics[0], "std_msgs/msg/String", "cdr", - "qos_profile1", "type_hash1"}; - rosbag2_storage::TopicMetadata topic_metadata_2 = {topics[1], "std_msgs/msg/String", "cdr", - "qos_profile2", "type_hash2"}; + rosbag2_storage::TopicMetadata topic_metadata_1 = { + topics[0], "std_msgs/msg/String", "cdr", {rclcpp::QoS(1)}, "type_hash1"}; + rosbag2_storage::TopicMetadata topic_metadata_2 = { + topics[1], "std_msgs/msg/String", "cdr", {rclcpp::QoS(2)}, "type_hash2"}; std::vector> @@ -116,8 +116,8 @@ TEST_F(McapStorageTestFixture, can_store_and_read_metadata_correctly) { auto writer = factory.open_read_write(options); - writer->create_topic({"topic1", "type1", "rmw1", "qos_profile1", "type_hash1"}, {}); - writer->create_topic({"topic2", "type2", "rmw2", "qos_profile2", "type_hash2"}, {}); + writer->create_topic({"topic1", "type1", "rmw1", {rclcpp::QoS(1)}, "type_hash1"}, {}); + writer->create_topic({"topic2", "type2", "rmw2", {rclcpp::QoS(2)}, "type_hash2"}, {}); (void)write_messages_to_mcap(messages, writer); auto metadata = writer->get_metadata(); metadata.ros_distro = "rolling"; @@ -137,10 +137,10 @@ TEST_F(McapStorageTestFixture, can_store_and_read_metadata_correctly) metadata.topics_with_message_count, UnorderedElementsAreArray({ rosbag2_storage::TopicInformation{ - rosbag2_storage::TopicMetadata{"topic2", "type2", "rmw2", "qos_profile2", "type_hash2"}, + rosbag2_storage::TopicMetadata{"topic2", "type2", "rmw2", {rclcpp::QoS(2)}, "type_hash2"}, 1u}, rosbag2_storage::TopicInformation{ - rosbag2_storage::TopicMetadata{"topic1", "type1", "rmw1", "qos_profile1", "type_hash1"}, + rosbag2_storage::TopicMetadata{"topic1", "type1", "rmw1", {rclcpp::QoS(1)}, "type_hash1"}, 2u}, })); EXPECT_THAT(metadata.message_count, Eq(3u)); @@ -180,7 +180,7 @@ TEST_F(TemporaryDirectoryFixture, can_write_and_read_basic_mcap_file) topic_metadata.name = topic_name; topic_metadata.type = "std_msgs/msg/String"; topic_metadata.serialization_format = "cdr"; - topic_metadata.offered_qos_profiles = "qos_profile1"; + topic_metadata.offered_qos_profiles = {rclcpp::QoS(1)}; topic_metadata.type_description_hash = "type_hash1"; std_msgs::msg::String msg; @@ -225,7 +225,7 @@ TEST_F(TemporaryDirectoryFixture, can_write_and_read_basic_mcap_file) EXPECT_THAT(topics_and_types, ElementsAreArray({rosbag2_storage::TopicMetadata{ - topic_name, "std_msgs/msg/String", "cdr", "qos_profile1", "type_hash1"}})); + topic_name, "std_msgs/msg/String", "cdr", {rclcpp::QoS(1)}, "type_hash1"}})); const auto metadata = reader->get_metadata(); @@ -233,8 +233,8 @@ TEST_F(TemporaryDirectoryFixture, can_write_and_read_basic_mcap_file) EXPECT_THAT(metadata.relative_file_paths, ElementsAreArray({expected_bag.string()})); EXPECT_THAT(metadata.topics_with_message_count, ElementsAreArray({rosbag2_storage::TopicInformation{ - rosbag2_storage::TopicMetadata{topic_name, "std_msgs/msg/String", "cdr", - "qos_profile1", "type_hash1"}, + rosbag2_storage::TopicMetadata{ + topic_name, "std_msgs/msg/String", "cdr", {rclcpp::QoS(1)}, "type_hash1"}, 1u}})); EXPECT_THAT(metadata.message_count, Eq(1u)); diff --git a/rosbag2_storage_sqlite3/src/rosbag2_storage_sqlite3/sqlite_storage.cpp b/rosbag2_storage_sqlite3/src/rosbag2_storage_sqlite3/sqlite_storage.cpp index d83e8d5883..a1e32ebee4 100644 --- a/rosbag2_storage_sqlite3/src/rosbag2_storage_sqlite3/sqlite_storage.cpp +++ b/rosbag2_storage_sqlite3/src/rosbag2_storage_sqlite3/sqlite_storage.cpp @@ -469,7 +469,7 @@ void SqliteStorage::create_topic( topic.name, topic.type, topic.serialization_format, - topic.offered_qos_profiles, + rosbag2_storage::serialize_rclcpp_qos_vector(topic.offered_qos_profiles), topic.type_description_hash); insert_topic->execute_and_reset(); topics_.emplace(topic.name, static_cast(database_->get_last_insert_id())); @@ -591,12 +591,16 @@ void SqliteStorage::fill_topics_and_types() std::string, std::string, std::string, std::string, std::string>(); for (auto result : query_results) { + auto offered_qos_profiles = rosbag2_storage::to_rclcpp_qos_vector( + // Before db_schema_version_ = 3 we didn't store metadata in the database and real + // metadata_.version will be lower than 9 + std::get<3>(result), (db_schema_version_ >= 3) ? metadata_.version : 8); all_topics_and_types_.push_back( { std::get<0>(result), std::get<1>(result), std::get<2>(result), - std::get<3>(result), + offered_qos_profiles, std::get<4>(result)}); } } else { @@ -606,8 +610,13 @@ void SqliteStorage::fill_topics_and_types() std::string, std::string, std::string, std::string>(); for (auto result : query_results) { + auto offered_qos_profiles = rosbag2_storage::to_rclcpp_qos_vector( + // Before db_schema_version_ = 3 we didn't store metadata in the database and real + // metadata_.version will be lower than 9 + std::get<3>(result), (db_schema_version_ >= 3) ? metadata_.version : 8); all_topics_and_types_.push_back( - {std::get<0>(result), std::get<1>(result), std::get<2>(result), std::get<3>(result), ""}); + {std::get<0>(result), std::get<1>(result), std::get<2>(result), offered_qos_profiles, + ""}); } } } else { @@ -617,7 +626,7 @@ void SqliteStorage::fill_topics_and_types() for (auto result : query_results) { all_topics_and_types_.push_back( - {std::get<0>(result), std::get<1>(result), std::get<2>(result), "", ""}); + {std::get<0>(result), std::get<1>(result), std::get<2>(result), {}, ""}); } } } @@ -681,10 +690,14 @@ void SqliteStorage::read_metadata() rcutils_time_point_value_t, std::string, std::string>(); for (auto result : query_results) { + auto offered_qos_profiles = rosbag2_storage::to_rclcpp_qos_vector( + // Before db_schema_version_ = 3 we didn't store metadata in the database and real + // metadata_.version will be lower than 9 + std::get<6>(result), (db_schema_version_ >= 3) ? metadata_.version : 8); metadata_.topics_with_message_count.push_back( { - {std::get<0>(result), std::get<1>(result), std::get<2>(result), std::get<6>( - result), std::get<7>(result)}, + {std::get<0>(result), std::get<1>(result), std::get<2>( + result), offered_qos_profiles, std::get<7>(result)}, static_cast(std::get<3>(result)) }); @@ -705,10 +718,14 @@ void SqliteStorage::read_metadata() rcutils_time_point_value_t, std::string>(); for (auto result : query_results) { + auto offered_qos_profiles = rosbag2_storage::to_rclcpp_qos_vector( + // Before db_schema_version_ = 3 we didn't store metadata in the database and real + // metadata_.version will be lower than 9 + std::get<6>(result), (db_schema_version_ >= 3) ? metadata_.version : 8); metadata_.topics_with_message_count.push_back( { - {std::get<0>(result), std::get<1>(result), std::get<2>(result), std::get<6>( - result), ""}, + {std::get<0>(result), std::get<1>(result), std::get<2>( + result), offered_qos_profiles, ""}, static_cast(std::get<3>(result)) }); @@ -732,7 +749,7 @@ void SqliteStorage::read_metadata() metadata_.topics_with_message_count.push_back( { {std::get<0>(result), std::get<1>(result), std::get<2>( - result), "", ""}, + result), {}, ""}, static_cast(std::get<3>(result)) }); diff --git a/rosbag2_storage_sqlite3/test/rosbag2_storage_sqlite3/storage_test_fixture.hpp b/rosbag2_storage_sqlite3/test/rosbag2_storage_sqlite3/storage_test_fixture.hpp index 57122e1224..5c99f9e9b2 100644 --- a/rosbag2_storage_sqlite3/test/rosbag2_storage_sqlite3/storage_test_fixture.hpp +++ b/rosbag2_storage_sqlite3/test/rosbag2_storage_sqlite3/storage_test_fixture.hpp @@ -110,7 +110,7 @@ class StorageTestFixture : public TemporaryDirectoryFixture std::string topic_name = std::get<2>(msg); std::string type_name = std::get<3>(msg); std::string rmw_format = std::get<4>(msg); - rw_storage.create_topic({topic_name, type_name, rmw_format, "", ""}, {}); + rw_storage.create_topic({topic_name, type_name, rmw_format, {}, ""}, {}); auto bag_message = std::make_shared(); bag_message->serialized_data = make_serialized_message(std::get<0>(msg)); bag_message->time_stamp = std::get<1>(msg); diff --git a/rosbag2_storage_sqlite3/test/rosbag2_storage_sqlite3/test_sqlite_storage.cpp b/rosbag2_storage_sqlite3/test/rosbag2_storage_sqlite3/test_sqlite_storage.cpp index ae8582d02f..9070d85bae 100644 --- a/rosbag2_storage_sqlite3/test/rosbag2_storage_sqlite3/test_sqlite_storage.cpp +++ b/rosbag2_storage_sqlite3/test/rosbag2_storage_sqlite3/test_sqlite_storage.cpp @@ -174,8 +174,8 @@ TEST_F(StorageTestFixture, get_all_topics_and_types_returns_the_correct_vector) const auto read_write_filename = (rcpputils::fs::path(temporary_dir_path_) / "rosbag").string(); writable_storage->open({read_write_filename, kPluginID}); - writable_storage->create_topic({"topic1", "type1", "rmw1", "qos_profile1", "type_hash1"}, {}); - writable_storage->create_topic({"topic2", "type2", "rmw2", "qos_profile2", "type_hash2"}, {}); + writable_storage->create_topic({"topic1", "type1", "rmw1", {rclcpp::QoS(1)}, "type_hash1"}, {}); + writable_storage->create_topic({"topic2", "type2", "rmw2", {rclcpp::QoS(2)}, "type_hash2"}, {}); const auto read_only_filename = writable_storage->get_relative_file_path(); @@ -190,8 +190,8 @@ TEST_F(StorageTestFixture, get_all_topics_and_types_returns_the_correct_vector) EXPECT_THAT( topics_and_types, ElementsAreArray( { - rosbag2_storage::TopicMetadata{"topic1", "type1", "rmw1", "qos_profile1", "type_hash1"}, - rosbag2_storage::TopicMetadata{"topic2", "type2", "rmw2", "qos_profile2", "type_hash2"} + rosbag2_storage::TopicMetadata{"topic1", "type1", "rmw1", {rclcpp::QoS(1)}, "type_hash1"}, + rosbag2_storage::TopicMetadata{"topic2", "type2", "rmw2", {rclcpp::QoS(2)}, "type_hash2"} })); } @@ -206,10 +206,10 @@ TEST_F(StorageTestFixture, get_all_message_definitions_returns_the_correct_vecto writable_storage->open({read_write_filename, kPluginID}); writable_storage->create_topic( - {"topic1", "type1", "rmw1", "qos_profile1", "type_hash1"}, + {"topic1", "type1", "rmw1", {rclcpp::QoS(1)}, "type_hash1"}, msg_definition); writable_storage->create_topic( - {"topic2", "type2", "rmw2", "qos_profile2", "type_hash2"}, + {"topic2", "type2", "rmw2", {rclcpp::QoS(2)}, "type_hash2"}, msg_definition); const auto read_only_filename = writable_storage->get_relative_file_path(); @@ -238,8 +238,8 @@ TEST_F(StorageTestFixture, get_metadata_returns_correct_struct) { auto writable_storage = std::make_shared(); auto read_write_filename = (rcpputils::fs::path(temporary_dir_path_) / "rosbag").string(); writable_storage->open({read_write_filename, kPluginID}); - writable_storage->create_topic({"topic1", "type1", "rmw1", "qos_profile1", "type_hash1"}, {}); - writable_storage->create_topic({"topic2", "type2", "rmw2", "qos_profile2", "type_hash2"}, {}); + writable_storage->create_topic({"topic1", "type1", "rmw1", {rclcpp::QoS(1)}, "type_hash1"}, {}); + writable_storage->create_topic({"topic2", "type2", "rmw2", {rclcpp::QoS(2)}, "type_hash2"}, {}); std::vector string_messages = {"first message", "second message", "third message"}; std::vector topics = {"topic1", "topic2"}; @@ -267,9 +267,9 @@ TEST_F(StorageTestFixture, get_metadata_returns_correct_struct) { metadata.topics_with_message_count, ElementsAreArray( { rosbag2_storage::TopicInformation{rosbag2_storage::TopicMetadata{ - "topic1", "type1", "rmw1", "qos_profile1", "type_hash1"}, 2u}, + "topic1", "type1", "rmw1", {rclcpp::QoS(1)}, "type_hash1"}, 2u}, rosbag2_storage::TopicInformation{rosbag2_storage::TopicMetadata{ - "topic2", "type2", "rmw2", "qos_profile1", "type_hash2"}, 1u} + "topic2", "type2", "rmw2", {rclcpp::QoS(2)}, "type_hash2"}, 1u} })); EXPECT_THAT(metadata.message_count, Eq(3u)); EXPECT_THAT( @@ -306,9 +306,9 @@ TEST_F(StorageTestFixture, get_metadata_returns_correct_struct_for_prefoxy_db_sc metadata.topics_with_message_count, ElementsAreArray( { rosbag2_storage::TopicInformation{rosbag2_storage::TopicMetadata{ - "topic1", "type1", "rmw_format", "", ""}, 2u}, + "topic1", "type1", "rmw_format", {}, ""}, 2u}, rosbag2_storage::TopicInformation{rosbag2_storage::TopicMetadata{ - "topic2", "type2", "rmw_format", "", ""}, 1u} + "topic2", "type2", "rmw_format", {}, ""}, 1u} })); EXPECT_THAT(metadata.message_count, Eq(3u)); EXPECT_THAT( @@ -405,8 +405,8 @@ TEST_F(StorageTestFixture, remove_topics_and_types_returns_the_empty_vector) { const auto read_write_filename = (rcpputils::fs::path(temporary_dir_path_) / "rosbag").string(); writable_storage->open({read_write_filename, kPluginID}); - writable_storage->create_topic({"topic1", "type1", "rmw1", "", "hash"}, {}); - writable_storage->remove_topic({"topic1", "type1", "rmw1", "", "hash"}); + writable_storage->create_topic({"topic1", "type1", "rmw1", {}, "hash"}, {}); + writable_storage->remove_topic({"topic1", "type1", "rmw1", {}, "hash"}); const auto read_only_filename = writable_storage->get_relative_file_path(); @@ -495,7 +495,7 @@ TEST_F(StorageTestFixture, storage_preset_profile_applies_over_defaults) { auto temp_dir = rcpputils::fs::path(temporary_dir_path_); const auto storage_uri = (temp_dir / "rosbag").string(); - rosbag2_storage::StorageOptions options{storage_uri, kPluginID, 0, 0, 0, "", ""}; + rosbag2_storage::StorageOptions options{storage_uri, kPluginID, 0, 0, 0, {}, ""}; options.storage_preset_profile = "resilient"; writable_storage->open(options, rosbag2_storage::storage_interfaces::IOFlag::READ_WRITE); diff --git a/rosbag2_transport/CMakeLists.txt b/rosbag2_transport/CMakeLists.txt index bcd5461478..d3f66e6c80 100644 --- a/rosbag2_transport/CMakeLists.txt +++ b/rosbag2_transport/CMakeLists.txt @@ -44,7 +44,6 @@ find_package(yaml_cpp_vendor REQUIRED) add_library(${PROJECT_NAME} SHARED src/rosbag2_transport/bag_rewrite.cpp src/rosbag2_transport/player.cpp - src/rosbag2_transport/qos.cpp src/rosbag2_transport/reader_writer_factory.cpp src/rosbag2_transport/recorder.cpp src/rosbag2_transport/record_options.cpp @@ -178,12 +177,6 @@ function(create_tests_for_rmw_implementation) LINK_LIBS rosbag2_transport AMENT_DEPS test_msgs rosbag2_test_common) - rosbag2_transport_add_gmock(test_qos - test/rosbag2_transport/test_qos.cpp - INCLUDE_DIRS $ - LINK_LIBS rosbag2_transport - AMENT_DEPS rosbag2_test_common yaml_cpp_vendor) - rosbag2_transport_add_gmock(test_record test/rosbag2_transport/test_record.cpp INCLUDE_DIRS $ diff --git a/rosbag2_transport/include/rosbag2_transport/qos.hpp b/rosbag2_transport/include/rosbag2_transport/qos.hpp deleted file mode 100644 index 9268bc4117..0000000000 --- a/rosbag2_transport/include/rosbag2_transport/qos.hpp +++ /dev/null @@ -1,88 +0,0 @@ -// 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 ROSBAG2_TRANSPORT__QOS_HPP_ -#define ROSBAG2_TRANSPORT__QOS_HPP_ - -#include -#include - -#include "rclcpp/node_interfaces/node_graph_interface.hpp" -#include "rclcpp/qos.hpp" - -#include "rosbag2_transport/visibility_control.hpp" -#include "rosbag2_storage/yaml.hpp" - -namespace rosbag2_transport -{ -/// Simple wrapper around rclcpp::QoS to provide a default constructor for YAML deserialization. -class ROSBAG2_TRANSPORT_PUBLIC Rosbag2QoS : public rclcpp::QoS -{ -public: - Rosbag2QoS() - : rclcpp::QoS(rmw_qos_profile_default.depth) {} - - explicit Rosbag2QoS(const rclcpp::QoS & value) - : rclcpp::QoS(value) {} - - Rosbag2QoS & default_history() - { - keep_last(rmw_qos_profile_default.depth); - return *this; - } - - // Create an adaptive QoS profile to use for subscribing to a set of offers from publishers. - /** - * - Uses rosbag2_transport defaults for History since they do not affect compatibility. - * - Adapts Durability and Reliability, falling back to the least strict publisher when there - * is a mixed offer. This behavior errs on the side of forming connections with all publishers. - * - Does not specify Lifespan, Deadline, or Liveliness to be maximally compatible, because - * these policies do not affect message delivery. - */ - static Rosbag2QoS adapt_request_to_offers( - const std::string & topic_name, - const std::vector & endpoints); - - // Create a QoS profile to offer for playback. - /** - * This logic exists because rosbag2 does not record on a per-publisher basis, so we try to - * get as close as possible to the original system's behavior, given a single publisher. - * If all profiles are the same (excepting History & Lifespan, which are purely local), - * that exact value is returned. - * Otherwise, fall back to the rosbag2 default and emit a warning. - */ - static Rosbag2QoS adapt_offer_to_recorded_offers( - const std::string & topic_name, - const std::vector & profiles); -}; -} // namespace rosbag2_transport - -namespace YAML -{ -template<> -struct ROSBAG2_TRANSPORT_PUBLIC convert -{ - static Node encode(const rmw_time_t & time); - static bool decode(const Node & node, rmw_time_t & time); -}; - -template<> -struct ROSBAG2_TRANSPORT_PUBLIC convert -{ - static Node encode(const rosbag2_transport::Rosbag2QoS & qos); - static bool decode(const Node & node, rosbag2_transport::Rosbag2QoS & qos); -}; -} // namespace YAML - -#endif // ROSBAG2_TRANSPORT__QOS_HPP_ diff --git a/rosbag2_transport/include/rosbag2_transport/record_options.hpp b/rosbag2_transport/include/rosbag2_transport/record_options.hpp index a387ab0998..50621877f8 100644 --- a/rosbag2_transport/include/rosbag2_transport/record_options.hpp +++ b/rosbag2_transport/include/rosbag2_transport/record_options.hpp @@ -58,7 +58,8 @@ template<> struct ROSBAG2_TRANSPORT_PUBLIC convert { static Node encode(const rosbag2_transport::RecordOptions & storage_options); - static bool decode(const Node & node, rosbag2_transport::RecordOptions & storage_options); + static bool decode( + const Node & node, rosbag2_transport::RecordOptions & storage_options, int version = 9); }; } // namespace YAML diff --git a/rosbag2_transport/src/rosbag2_transport/bag_rewrite.cpp b/rosbag2_transport/src/rosbag2_transport/bag_rewrite.cpp index ebf47f1b87..0ea9771b10 100644 --- a/rosbag2_transport/src/rosbag2_transport/bag_rewrite.cpp +++ b/rosbag2_transport/src/rosbag2_transport/bag_rewrite.cpp @@ -81,7 +81,7 @@ setup_topic_filtering( { std::unordered_map> filtered_outputs; std::map> input_topics; - std::unordered_map input_topics_qos_profiles; + std::unordered_map> input_topics_qos_profiles; std::unordered_map input_topics_serialization_format; // message_definitions_map mapping topic_type to message_definition std::unordered_map message_definitions_map; @@ -96,11 +96,11 @@ setup_topic_filtering( // Gather all offered qos profiles from all inputs input_topics_qos_profiles.try_emplace(topic_name); - YAML::Node & all_offered = input_topics_qos_profiles[topic_name]; - YAML::Node offered_qos_profiles = YAML::Load(topic_metadata.offered_qos_profiles); - for (auto qos : offered_qos_profiles) { - all_offered.push_back(qos); - } + input_topics_qos_profiles[topic_name].insert( + input_topics_qos_profiles[topic_name].end(), + topic_metadata.offered_qos_profiles.begin(), + topic_metadata.offered_qos_profiles.end() + ); } // Fill message_definitions_map std::vector msg_definitions; @@ -127,9 +127,7 @@ setup_topic_filtering( topic_metadata.serialization_format = record_options.rmw_serialization_format; } - std::stringstream qos_profiles; - qos_profiles << input_topics_qos_profiles[topic_name]; - topic_metadata.offered_qos_profiles = qos_profiles.str(); + topic_metadata.offered_qos_profiles = input_topics_qos_profiles[topic_name]; auto message_definition_ptr = message_definitions_map.find(topic_type); if (message_definition_ptr != message_definitions_map.end()) { diff --git a/rosbag2_transport/src/rosbag2_transport/player.cpp b/rosbag2_transport/src/rosbag2_transport/player.cpp index 98c767e24e..8c37e420e3 100644 --- a/rosbag2_transport/src/rosbag2_transport/player.cpp +++ b/rosbag2_transport/src/rosbag2_transport/player.cpp @@ -35,7 +35,7 @@ #include "rosbag2_storage/storage_filter.hpp" -#include "rosbag2_transport/qos.hpp" +#include "rosbag2_storage/qos.hpp" namespace { @@ -68,7 +68,7 @@ rclcpp::QoS publisher_qos_for_topic( const std::unordered_map & topic_qos_profile_overrides, const rclcpp::Logger & logger) { - using rosbag2_transport::Rosbag2QoS; + using rosbag2_storage::Rosbag2QoS; auto qos_it = topic_qos_profile_overrides.find(topic.name); if (qos_it != topic_qos_profile_overrides.end()) { RCLCPP_INFO_STREAM( @@ -79,9 +79,9 @@ rclcpp::QoS publisher_qos_for_topic( return Rosbag2QoS{}; } - const auto profiles_yaml = YAML::Load(topic.offered_qos_profiles); - const auto offered_qos_profiles = profiles_yaml.as>(); - return Rosbag2QoS::adapt_offer_to_recorded_offers(topic.name, offered_qos_profiles); + return Rosbag2QoS::adapt_offer_to_recorded_offers( + topic.name, + rosbag2_storage::from_rclcpp_qos_vector(topic.offered_qos_profiles)); } } // namespace diff --git a/rosbag2_transport/src/rosbag2_transport/qos.cpp b/rosbag2_transport/src/rosbag2_transport/qos.cpp deleted file mode 100644 index f643274a86..0000000000 --- a/rosbag2_transport/src/rosbag2_transport/qos.cpp +++ /dev/null @@ -1,227 +0,0 @@ -// 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 "rosbag2_transport/qos.hpp" -#include "logging.hpp" - -namespace -{ -/** - * The following constants were the "Inifinity" value returned by RMW implementations before - * the introduction of RMW_DURATION_INFINITE and associated RMW fixes - * RMW: https://github.com/ros2/rmw/pull/301 - * Fast-DDS: https://github.com/ros2/rmw_fastrtps/pull/515 - * CycloneDDS: https://github.com/ros2/rmw_cyclonedds/pull/288 - * RTI Connext: https://github.com/ros2/rmw_connext/pull/491 - * - * These values exist in bags recorded in Foxy, they need to be translated to RMW_DURATION_INFINITE - * to be consistently understood for playback. - * With those values, if a bag is played back in a different implementation than it was recorded, - * the publishers will fail to be created with an error indicating an invalid QoS value.. - */ -static const rmw_time_t RMW_CYCLONEDDS_FOXY_INFINITE = rmw_time_from_nsec(0x7FFFFFFFFFFFFFFFll); -static const rmw_time_t RMW_FASTRTPS_FOXY_INFINITE {0x7FFFFFFFll, 0xFFFFFFFFll}; -static const rmw_time_t RMW_CONNEXT_FOXY_INFINITE {0x7FFFFFFFll, 0x7FFFFFFFll}; -} // namespace - -namespace YAML -{ -Node convert::encode(const rmw_time_t & time) -{ - Node node; - node["sec"] = time.sec; - node["nsec"] = time.nsec; - return node; -} - -bool convert::decode(const Node & node, rmw_time_t & time) -{ - time.sec = node["sec"].as(); - time.nsec = node["nsec"].as(); - if ( - rmw_time_equal(time, RMW_CYCLONEDDS_FOXY_INFINITE) || - rmw_time_equal(time, RMW_FASTRTPS_FOXY_INFINITE) || - rmw_time_equal(time, RMW_CONNEXT_FOXY_INFINITE)) - { - time = RMW_DURATION_INFINITE; - } - return true; -} - -Node convert::encode(const rosbag2_transport::Rosbag2QoS & qos) -{ - const auto & p = qos.get_rmw_qos_profile(); - Node node; - node["history"] = static_cast(p.history); - node["depth"] = p.depth; - node["reliability"] = static_cast(p.reliability); - node["durability"] = static_cast(p.durability); - node["deadline"] = p.deadline; - node["lifespan"] = p.lifespan; - node["liveliness"] = static_cast(p.liveliness); - node["liveliness_lease_duration"] = p.liveliness_lease_duration; - node["avoid_ros_namespace_conventions"] = p.avoid_ros_namespace_conventions; - return node; -} - -bool convert::decode( - const Node & node, rosbag2_transport::Rosbag2QoS & qos) -{ - auto history = static_cast(node["history"].as()); - auto reliability = static_cast(node["reliability"].as()); - auto durability = static_cast(node["durability"].as()); - auto liveliness = static_cast(node["liveliness"].as()); - - qos - .keep_last(node["depth"].as()) - .history(history) - .reliability(reliability) - .durability(durability) - .deadline(node["deadline"].as()) - .lifespan(node["lifespan"].as()) - .liveliness(liveliness) - .liveliness_lease_duration(node["liveliness_lease_duration"].as()) - .avoid_ros_namespace_conventions(node["avoid_ros_namespace_conventions"].as()); - return true; -} -} // namespace YAML - -namespace rosbag2_transport -{ -Rosbag2QoS Rosbag2QoS::adapt_request_to_offers( - const std::string & topic_name, const std::vector & endpoints) -{ - if (endpoints.empty()) { - return Rosbag2QoS{}; - } - size_t num_endpoints = endpoints.size(); - size_t reliability_reliable_endpoints_count = 0; - size_t durability_transient_local_endpoints_count = 0; - for (const auto & endpoint : endpoints) { - const auto & profile = endpoint.qos_profile().get_rmw_qos_profile(); - if (profile.reliability == RMW_QOS_POLICY_RELIABILITY_RELIABLE) { - reliability_reliable_endpoints_count++; - } - if (profile.durability == RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL) { - durability_transient_local_endpoints_count++; - } - } - - // We set policies in order as defined in rmw_qos_profile_t - Rosbag2QoS request_qos{}; - // Policy: history, depth - // History does not affect compatibility - - // Policy: reliability - if (reliability_reliable_endpoints_count == num_endpoints) { - request_qos.reliable(); - } else { - if (reliability_reliable_endpoints_count > 0) { - ROSBAG2_TRANSPORT_LOG_WARN_STREAM( - "Some, but not all, publishers on topic \"" << topic_name << "\" " - "are offering RMW_QOS_POLICY_RELIABILITY_RELIABLE. " - "Falling back to RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT " - "as it will connect to all publishers. " - "Some messages from Reliable publishers could be dropped."); - } - request_qos.best_effort(); - } - - // Policy: durability - // If all publishers offer transient_local, we can request it and receive latched messages - if (durability_transient_local_endpoints_count == num_endpoints) { - request_qos.transient_local(); - } else { - if (durability_transient_local_endpoints_count > 0) { - ROSBAG2_TRANSPORT_LOG_WARN_STREAM( - "Some, but not all, publishers on topic \"" << topic_name << "\" " - "are offering RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL. " - "Falling back to RMW_QOS_POLICY_DURABILITY_VOLATILE " - "as it will connect to all publishers. " - "Previously-published latched messages will not be retrieved."); - } - request_qos.durability_volatile(); - } - // Policy: deadline - // Deadline does not affect delivery of messages, - // and we do not record Deadline"Missed events. - // We can always use unspecified deadline, which will be compatible with all publishers. - - // Policy: lifespan - // Lifespan does not affect compatibiliy - - // Policy: liveliness, liveliness_lease_duration - // Liveliness does not affect delivery of messages, - // and we do not record LivelinessChanged events. - // We can always use unspecified liveliness, which will be compatible with all publishers. - return request_qos; -} - -namespace -{ -bool operator==(const rmw_time_t & lhs, const rmw_time_t & rhs) -{ - return lhs.sec == rhs.sec && lhs.nsec == rhs.nsec; -} - -/** Check if all QoS profiles in the vector are identical when only looking at - * policies that affect compatibility. - * This means it excludes history and lifespan from the equality check. - */ -bool all_profiles_effectively_same(const std::vector & profiles) -{ - auto iterator = profiles.begin(); - const auto p_ref = iterator->get_rmw_qos_profile(); - iterator++; - for (; iterator != profiles.end(); iterator++) { - const auto p_next = iterator->get_rmw_qos_profile(); - bool compatibility_equals_previous = ( - // excluding history - p_ref.reliability == p_next.reliability && - p_ref.durability == p_next.durability && - p_ref.deadline == p_next.deadline && - // excluding lifespan - p_ref.liveliness == p_next.liveliness && - p_ref.liveliness_lease_duration == p_next.liveliness_lease_duration - ); - if (!compatibility_equals_previous) { - return false; - } - } - return true; -} -} // unnamed namespace - -Rosbag2QoS Rosbag2QoS::adapt_offer_to_recorded_offers( - const std::string & topic_name, const std::vector & profiles) -{ - if (profiles.empty()) { - return Rosbag2QoS{}; - } - if (all_profiles_effectively_same(profiles)) { - auto result = profiles[0]; - return result.default_history(); - } - - ROSBAG2_TRANSPORT_LOG_WARN_STREAM( - "Not all original publishers on topic " << topic_name << " offered the same QoS profiles. " - "Rosbag2 cannot yet choose an adapted profile to offer for this mixed case. " - "Falling back to the rosbag2_transport default publisher offer."); - return Rosbag2QoS{}; -} -} // namespace rosbag2_transport diff --git a/rosbag2_transport/src/rosbag2_transport/record_options.cpp b/rosbag2_transport/src/rosbag2_transport/record_options.cpp index 9f94ac68a3..383b97d568 100644 --- a/rosbag2_transport/src/rosbag2_transport/record_options.cpp +++ b/rosbag2_transport/src/rosbag2_transport/record_options.cpp @@ -16,7 +16,7 @@ #include #include -#include "rosbag2_transport/qos.hpp" +#include "rosbag2_storage/qos.hpp" #include "rosbag2_transport/record_options.hpp" namespace YAML @@ -54,17 +54,18 @@ Node convert::encode( node["compression_format"] = record_options.compression_format; node["compression_queue_size"] = record_options.compression_queue_size; node["compression_threads"] = record_options.compression_threads; - std::map qos_overrides( + std::map qos_overrides( record_options.topic_qos_profile_overrides.begin(), record_options.topic_qos_profile_overrides.end()); - node["topic_qos_profile_overrides"] = qos_overrides; + node["topic_qos_profile_overrides"] = convert>::encode(qos_overrides); node["include_hidden_topics"] = record_options.include_hidden_topics; node["include_unpublished_topics"] = record_options.include_unpublished_topics; return node; } bool convert::decode( - const Node & node, rosbag2_transport::RecordOptions & record_options) + const Node & node, rosbag2_transport::RecordOptions & record_options, int version) { optional_assign(node, "all", record_options.all); optional_assign(node, "is_discovery_disabled", record_options.is_discovery_disabled); @@ -82,9 +83,11 @@ bool convert::decode( optional_assign(node, "compression_threads", record_options.compression_threads); // yaml-cpp doesn't implement unordered_map - std::map qos_overrides; - optional_assign>( - node, "topic_qos_profile_overrides", qos_overrides); + std::map qos_overrides; + if (node["topic_qos_profile_overrides"]) { + qos_overrides = YAML::decode_for_version>( + node["topic_qos_profile_overrides"], version); + } record_options.topic_qos_profile_overrides.insert(qos_overrides.begin(), qos_overrides.end()); optional_assign(node, "include_hidden_topics", record_options.include_hidden_topics); diff --git a/rosbag2_transport/src/rosbag2_transport/recorder.cpp b/rosbag2_transport/src/rosbag2_transport/recorder.cpp index 69c280e136..c3baafeff3 100644 --- a/rosbag2_transport/src/rosbag2_transport/recorder.cpp +++ b/rosbag2_transport/src/rosbag2_transport/recorder.cpp @@ -35,7 +35,7 @@ #include "rosbag2_interfaces/srv/snapshot.hpp" #include "rosbag2_storage/yaml.hpp" -#include "rosbag2_transport/qos.hpp" +#include "rosbag2_storage/qos.hpp" #include "logging.hpp" #include "rosbag2_transport/topic_filter.hpp" @@ -110,8 +110,8 @@ class RecorderImpl */ rclcpp::QoS subscription_qos_for_topic(const std::string & topic_name) const; - // Serialize all currently offered QoS profiles for a topic into a YAML list. - std::string serialized_offered_qos_profiles_for_topic( + // Get all currently offered QoS profiles for a topic. + std::vector offered_qos_profiles_for_topic( const std::vector & topics_endpoint_info) const; void warn_if_new_qos_for_subscribed_topic(const std::string & topic_name); @@ -461,7 +461,7 @@ void RecorderImpl::subscribe_topics( topic_with_type.first, topic_with_type.second, serialization_format_, - serialized_offered_qos_profiles_for_topic(endpoint_infos), + offered_qos_profiles_for_topic(endpoint_infos), type_description_hash_for_topic(endpoint_infos), }); } @@ -474,7 +474,7 @@ void RecorderImpl::subscribe_topic(const rosbag2_storage::TopicMetadata & topic) // that callback called before we reached out the line: writer_->create_topic(topic) writer_->create_topic(topic); - Rosbag2QoS subscription_qos{subscription_qos_for_topic(topic.name)}; + rosbag2_storage::Rosbag2QoS subscription_qos{subscription_qos_for_topic(topic.name)}; auto subscription = create_subscription(topic.name, topic.type, subscription_qos); if (subscription) { subscriptions_.insert({topic.name, subscription}); @@ -503,14 +503,14 @@ RecorderImpl::create_subscription( return subscription; } -std::string RecorderImpl::serialized_offered_qos_profiles_for_topic( +std::vector RecorderImpl::offered_qos_profiles_for_topic( const std::vector & topics_endpoint_info) const { - YAML::Node offered_qos_profiles; + std::vector offered_qos_profiles; for (const auto & info : topics_endpoint_info) { - offered_qos_profiles.push_back(Rosbag2QoS(info.qos_profile())); + offered_qos_profiles.push_back(info.qos_profile()); } - return YAML::Dump(offered_qos_profiles); + return offered_qos_profiles; } std::string type_hash_to_string(const rosidl_type_hash_t & type_hash) @@ -576,7 +576,7 @@ rclcpp::QoS RecorderImpl::subscription_qos_for_topic(const std::string & topic_n "Overriding subscription profile for " << topic_name); return topic_qos_profile_overrides_.at(topic_name); } - return Rosbag2QoS::adapt_request_to_offers( + return rosbag2_storage::Rosbag2QoS::adapt_request_to_offers( topic_name, node->get_publishers_info_by_topic(topic_name)); } diff --git a/rosbag2_transport/test/rosbag2_transport/rosbag2_play_duration_until_fixture.hpp b/rosbag2_transport/test/rosbag2_transport/rosbag2_play_duration_until_fixture.hpp index 20b969da7f..7366acd5f8 100644 --- a/rosbag2_transport/test/rosbag2_transport/rosbag2_play_duration_until_fixture.hpp +++ b/rosbag2_transport/test/rosbag2_transport/rosbag2_play_duration_until_fixture.hpp @@ -91,8 +91,8 @@ class RosBag2PlayDurationAndUntilTestFixture : public RosBag2PlayTestFixture std::vector get_topic_types() { - return {{kTopic1Name_, "test_msgs/BasicTypes", "", "", ""}, - {kTopic2Name_, "test_msgs/Arrays", "", "", ""}}; + return {{kTopic1Name_, "test_msgs/BasicTypes", "", {}, ""}, + {kTopic2Name_, "test_msgs/Arrays", "", {}, ""}}; } std::vector> diff --git a/rosbag2_transport/test/rosbag2_transport/test_burst.cpp b/rosbag2_transport/test/rosbag2_transport/test_burst.cpp index 3e666b56ad..f603aba386 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_burst.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_burst.cpp @@ -35,7 +35,7 @@ TEST_F(RosBag2PlayTestFixture, burst_with_false_preconditions) { primitive_message->int32_value = 42; auto topic_types = std::vector{ - {"topic1", "test_msgs/BasicTypes", "", "", ""}}; + {"topic1", "test_msgs/BasicTypes", "", {}, ""}}; std::vector> messages = {serialize_test_message("topic1", 2100, primitive_message)}; @@ -56,7 +56,7 @@ TEST_F(RosBag2PlayTestFixture, burst_bursts_requested_messages_without_delays) { primitive_message->int32_value = 42; auto topic_types = std::vector{ - {"topic1", "test_msgs/BasicTypes", "", "", ""}}; + {"topic1", "test_msgs/BasicTypes", "", {}, ""}}; std::vector> messages = { @@ -109,7 +109,7 @@ TEST_F(RosBag2PlayTestFixture, burst_stops_at_end_of_file) { primitive_message->int32_value = 42; auto topic_types = std::vector{ - {"topic1", "test_msgs/BasicTypes", "", "", ""}}; + {"topic1", "test_msgs/BasicTypes", "", {}, ""}}; std::vector> messages = { @@ -156,7 +156,7 @@ TEST_F(RosBag2PlayTestFixture, burst_bursting_one_by_one_messages_with_the_same_ primitive_message->int32_value = 42; auto topic_types = std::vector{ - {"topic1", "test_msgs/BasicTypes", "", "", ""}}; + {"topic1", "test_msgs/BasicTypes", "", {}, ""}}; std::vector> messages = { @@ -209,7 +209,7 @@ TEST_F(RosBag2PlayTestFixture, play_respect_messages_timing_after_burst) { primitive_message->int32_value = 42; auto topic_types = std::vector{ - {"topic1", "test_msgs/BasicTypes", "", "", ""}}; + {"topic1", "test_msgs/BasicTypes", "", {}, ""}}; std::vector> messages = { @@ -264,7 +264,7 @@ TEST_F(RosBag2PlayTestFixture, player_can_resume_after_burst) { primitive_message->int32_value = 42; auto topic_types = std::vector{ - {"topic1", "test_msgs/BasicTypes", "", "", ""}}; + {"topic1", "test_msgs/BasicTypes", "", {}, ""}}; std::vector> messages = { @@ -312,8 +312,8 @@ TEST_F(RosBag2PlayTestFixture, burst_bursting_only_filtered_topics) { complex_message1->bool_values = {{true, false, true}}; auto topic_types = std::vector{ - {"topic1", "test_msgs/BasicTypes", "", "", ""}, - {"topic2", "test_msgs/Arrays", "", "", ""}, + {"topic1", "test_msgs/BasicTypes", "", {}, ""}, + {"topic2", "test_msgs/Arrays", "", {}, ""}, }; std::vector> messages = diff --git a/rosbag2_transport/test/rosbag2_transport/test_keyboard_controls.cpp b/rosbag2_transport/test/rosbag2_transport/test_keyboard_controls.cpp index 8e91d01763..6e83b0911d 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_keyboard_controls.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_keyboard_controls.cpp @@ -98,7 +98,7 @@ TEST_F(RosBag2PlayTestFixture, invalid_keybindings) auto message_time_difference = rclcpp::Duration(1, 0); auto topics_and_types = - std::vector{{"topic1", "test_msgs/Strings", "", "", ""}}; + std::vector{{"topic1", "test_msgs/Strings", "", {}, ""}}; std::vector> messages = {serialize_test_message("topic1", 0, primitive_message1), serialize_test_message("topic1", 0, primitive_message2)}; @@ -132,7 +132,7 @@ TEST_F(RosBag2PlayTestFixture, test_keyboard_controls) auto message_time_difference = rclcpp::Duration(1, 0); auto topics_and_types = - std::vector{{"topic1", "test_msgs/Strings", "", "", ""}}; + std::vector{{"topic1", "test_msgs/Strings", "", {}, ""}}; std::vector> messages = {serialize_test_message("topic1", 0, primitive_message1), serialize_test_message("topic1", 0, primitive_message2), diff --git a/rosbag2_transport/test/rosbag2_transport/test_play.cpp b/rosbag2_transport/test/rosbag2_transport/test_play.cpp index 7b33abbcdc..afda51a574 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_play.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_play.cpp @@ -32,7 +32,7 @@ #include "test_msgs/msg/basic_types.hpp" #include "test_msgs/message_fixtures.hpp" -#include "rosbag2_transport/qos.hpp" +#include "rosbag2_storage/qos.hpp" #include "rosbag2_play_test_fixture.hpp" #include "rosbag2_transport_test_fixture.hpp" @@ -53,8 +53,8 @@ TEST_F(RosBag2PlayTestFixture, recorded_messages_are_played_for_all_topics) complex_message1->bool_values = {{true, false, true}}; auto topic_types = std::vector{ - {"topic1", "test_msgs/BasicTypes", "", "", ""}, - {"topic2", "test_msgs/Arrays", "", "", ""}, + {"topic1", "test_msgs/BasicTypes", "", {}, ""}, + {"topic2", "test_msgs/Arrays", "", {}, ""}, }; std::vector> messages = @@ -122,9 +122,9 @@ TEST_F(RosBag2PlayTestFixture, recorded_messages_are_played_for_all_topics_with_ unknown_message1->int32_value = 42; auto topic_types = std::vector{ - {"topic1", "test_msgs/BasicTypes", "", "", ""}, - {"topic2", "test_msgs/Arrays", "", "", ""}, - {"topic3", "unknown_msgs/UnknownType", "", "", ""}, + {"topic1", "test_msgs/BasicTypes", "", {}, ""}, + {"topic2", "test_msgs/Arrays", "", {}, ""}, + {"topic3", "unknown_msgs/UnknownType", "", {}, ""}, }; std::vector> messages = @@ -190,8 +190,8 @@ TEST_F(RosBag2PlayTestFixture, recorded_messages_are_played_for_filtered_topics) complex_message1->bool_values = {{true, false, true}}; auto topic_types = std::vector{ - {"topic1", "test_msgs/BasicTypes", "", "", ""}, - {"topic2", "test_msgs/Arrays", "", "", ""}, + {"topic1", "test_msgs/BasicTypes", "", {}, ""}, + {"topic2", "test_msgs/Arrays", "", {}, ""}, }; std::vector> messages = @@ -315,9 +315,9 @@ TEST_F(RosBag2PlayTestFixture, recorded_messages_are_played_for_filtered_topics_ unknown_message1->int32_value = 42; auto topic_types = std::vector{ - {"topic1", "test_msgs/BasicTypes", "", "", ""}, - {"topic2", "test_msgs/Arrays", "", "", ""}, - {"topic3", "unknown_msgs/UnknownType", "", "", ""}, + {"topic1", "test_msgs/BasicTypes", "", {}, ""}, + {"topic2", "test_msgs/Arrays", "", {}, ""}, + {"topic3", "unknown_msgs/UnknownType", "", {}, ""}, }; std::vector> messages = @@ -424,7 +424,7 @@ TEST_F(RosBag2PlayTestFixture, player_gracefully_exit_by_rclcpp_shutdown_in_paus auto primitive_message1 = get_messages_basic_types()[0]; primitive_message1->int32_value = 42; auto topic_types = std::vector{ - {"topic1", "test_msgs/BasicTypes", "", "", ""}, + {"topic1", "test_msgs/BasicTypes", "", {}, ""}, }; std::vector> messages = @@ -450,12 +450,14 @@ TEST_F(RosBag2PlayTestFixture, player_gracefully_exit_by_rclcpp_shutdown_in_paus class RosBag2PlayQosOverrideTestFixture : public RosBag2PlayTestFixture { public: + using Rosbag2QoS = rosbag2_storage::Rosbag2QoS; + RosBag2PlayQosOverrideTestFixture() : RosBag2PlayTestFixture() { } - void initialize(const std::vector & offered_qos) + void initialize(const std::vector & offered_qos) { // Because these tests only cares about compatibility (receiving any messages at all) // We publish one more message than we expect to receive, to avoid caring about @@ -467,16 +469,13 @@ class RosBag2PlayQosOverrideTestFixture : public RosBag2PlayTestFixture messages_.push_back(serialize_test_message(topic_name_, timestamp, basic_msg_)); } - std::string serialized_offered_qos = ""; - if (!offered_qos.empty()) { - YAML::Node offered_qos_yaml; - for (const auto & profile : offered_qos) { - offered_qos_yaml.push_back(profile); - } - serialized_offered_qos = YAML::Dump(offered_qos_yaml); - } topic_types_.push_back( - {topic_name_, msg_type_, "" /*serialization_format*/, serialized_offered_qos, ""}); + { + topic_name_, + msg_type_, + "", + offered_qos, + ""}); } template @@ -636,7 +635,7 @@ TEST_F(RosBag2PlayQosOverrideTestFixture, override_has_precedence_over_recorded) TEST_F(RosBag2PlayTestFixture, read_split_callback_is_called) { auto topic_types = std::vector{ - {"topic1", "test_msgs/BasicTypes", "", "", ""}, + {"topic1", "test_msgs/BasicTypes", "", {}, ""}, }; auto prepared_mock_reader = std::make_unique(); diff --git a/rosbag2_transport/test/rosbag2_transport/test_play_callbacks.cpp b/rosbag2_transport/test/rosbag2_transport/test_play_callbacks.cpp index 1be6eaf97e..485161d967 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_play_callbacks.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_play_callbacks.cpp @@ -39,7 +39,7 @@ class Rosbag2PlayCallbacksTestFixture : public Rosbag2TransportTestFixture rclcpp::init(0, nullptr); auto topics_and_types = - std::vector{{"topic1", "test_msgs/Strings", "", "", ""}}; + std::vector{{"topic1", "test_msgs/Strings", "", {}, ""}}; std::vector> messages; messages.reserve(num_test_messages_); diff --git a/rosbag2_transport/test/rosbag2_transport/test_play_duration.cpp b/rosbag2_transport/test/rosbag2_transport/test_play_duration.cpp index 5cad8afdcb..faec91723a 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_play_duration.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_play_duration.cpp @@ -62,7 +62,7 @@ TEST_F(RosBag2PlayDurationTestFixture, play_for_none_are_played_due_to_duration) primitive_message2->int32_value = 2; auto topic_types = std::vector{ - {kTopic1Name_, "test_msgs/BasicTypes", "", "", ""}}; + {kTopic1Name_, "test_msgs/BasicTypes", "", {}, ""}}; std::vector> messages = { @@ -105,7 +105,7 @@ TEST_F(RosBag2PlayDurationTestFixture, play_for_less_than_the_total_duration) primitive_message2->int32_value = 2; auto topic_types = std::vector{ - {kTopic1Name_, "test_msgs/BasicTypes", "", "", ""}}; + {kTopic1Name_, "test_msgs/BasicTypes", "", {}, ""}}; std::vector> messages = { @@ -172,7 +172,7 @@ TEST_F( TEST_F(RosBag2PlayDurationTestFixture, play_should_return_false_when_interrupted) { auto topic_types = std::vector{ - {kTopic1Name_, "test_msgs/BasicTypes", "", "", ""}}; + {kTopic1Name_, "test_msgs/BasicTypes", "", {}, ""}}; auto primitive_message = get_messages_basic_types()[0]; primitive_message->int32_value = kIntValue; diff --git a/rosbag2_transport/test/rosbag2_transport/test_play_loop.cpp b/rosbag2_transport/test/rosbag2_transport/test_play_loop.cpp index c35ce0e1b6..2f9bd1c2d6 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_play_loop.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_play_loop.cpp @@ -52,7 +52,7 @@ TEST_F(RosBag2PlayTestFixture, play_bag_file_twice) { primitive_message1->int32_value = test_value; auto topic_types = std::vector{ - {"loop_test_topic", "test_msgs/BasicTypes", "", "", ""} + {"loop_test_topic", "test_msgs/BasicTypes", "", {}, ""} }; std::vector> messages(num_messages, @@ -114,7 +114,7 @@ TEST_F(RosBag2PlayTestFixture, messages_played_in_loop) { primitive_message1->int32_value = test_value; auto topic_types = std::vector{ - {"loop_test_topic", "test_msgs/BasicTypes", "", "", ""} + {"loop_test_topic", "test_msgs/BasicTypes", "", {}, ""} }; std::vector> messages(num_messages, diff --git a/rosbag2_transport/test/rosbag2_transport/test_play_next.cpp b/rosbag2_transport/test/rosbag2_transport/test_play_next.cpp index 8e47e2d05e..eba1c6d60f 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_play_next.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_play_next.cpp @@ -35,7 +35,7 @@ TEST_F(RosBag2PlayTestFixture, play_next_with_false_preconditions) { primitive_message->int32_value = 42; auto topic_types = std::vector{ - {"topic1", "test_msgs/BasicTypes", "", "", ""}}; + {"topic1", "test_msgs/BasicTypes", "", {}, ""}}; std::vector> messages = {serialize_test_message("topic1", 2100, primitive_message)}; @@ -56,7 +56,7 @@ TEST_F(RosBag2PlayTestFixture, play_next_playing_all_messages_without_delays) { primitive_message->int32_value = 42; auto topic_types = std::vector{ - {"topic1", "test_msgs/BasicTypes", "", "", ""}}; + {"topic1", "test_msgs/BasicTypes", "", {}, ""}}; std::vector> messages = { @@ -110,7 +110,7 @@ TEST_F(RosBag2PlayTestFixture, play_next_playing_one_by_one_messages_with_the_sa primitive_message->int32_value = 42; auto topic_types = std::vector{ - {"topic1", "test_msgs/BasicTypes", "", "", ""}}; + {"topic1", "test_msgs/BasicTypes", "", {}, ""}}; std::vector> messages = { @@ -163,7 +163,7 @@ TEST_F(RosBag2PlayTestFixture, play_respect_messages_timing_after_play_next) { primitive_message->int32_value = 42; auto topic_types = std::vector{ - {"topic1", "test_msgs/BasicTypes", "", "", ""}}; + {"topic1", "test_msgs/BasicTypes", "", {}, ""}}; std::vector> messages = { @@ -218,7 +218,7 @@ TEST_F(RosBag2PlayTestFixture, player_can_resume_after_play_next) { primitive_message->int32_value = 42; auto topic_types = std::vector{ - {"topic1", "test_msgs/BasicTypes", "", "", ""}}; + {"topic1", "test_msgs/BasicTypes", "", {}, ""}}; std::vector> messages = { @@ -266,8 +266,8 @@ TEST_F(RosBag2PlayTestFixture, play_next_playing_only_filtered_topics) { complex_message1->bool_values = {{true, false, true}}; auto topic_types = std::vector{ - {"topic1", "test_msgs/BasicTypes", "", "", ""}, - {"topic2", "test_msgs/Arrays", "", "", ""}, + {"topic1", "test_msgs/BasicTypes", "", {}, ""}, + {"topic2", "test_msgs/Arrays", "", {}, ""}, }; std::vector> messages = diff --git a/rosbag2_transport/test/rosbag2_transport/test_play_publish_clock.cpp b/rosbag2_transport/test/rosbag2_transport/test_play_publish_clock.cpp index 4299d69137..ceae214f3b 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_play_publish_clock.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_play_publish_clock.cpp @@ -43,7 +43,7 @@ class ClockPublishFixture : public RosBag2PlayTestFixture { // Fake bag setup auto topic_types = std::vector{ - {"topic1", "test_msgs/BasicTypes", "", "", ""}, + {"topic1", "test_msgs/BasicTypes", "", {}, ""}, }; std::vector> messages; diff --git a/rosbag2_transport/test/rosbag2_transport/test_play_seek.cpp b/rosbag2_transport/test/rosbag2_transport/test_play_seek.cpp index de3b94b893..eef318a014 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_play_seek.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_play_seek.cpp @@ -42,7 +42,7 @@ class RosBag2PlaySeekTestFixture : RosBag2PlayTestFixture() { topic_types_ = std::vector{ - {"topic1", "test_msgs/BasicTypes", rmw_get_serialization_format(), "", ""}}; + {"topic1", "test_msgs/BasicTypes", rmw_get_serialization_format(), {}, ""}}; const rcpputils::fs::path base{_SRC_RESOURCES_DIR_PATH}; const rcpputils::fs::path bag_path = base / GetParam() / "test_bag_for_seek"; diff --git a/rosbag2_transport/test/rosbag2_transport/test_play_services.cpp b/rosbag2_transport/test/rosbag2_transport/test_play_services.cpp index 20c032782f..e21289b8bb 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_play_services.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_play_services.cpp @@ -197,7 +197,7 @@ class PlaySrvsTest : public RosBag2PlayTestFixture message->int32_value = 42; auto topic_types = std::vector{ - {test_topic_, "test_msgs/BasicTypes", "", "", ""}, + {test_topic_, "test_msgs/BasicTypes", "", {}, ""}, }; std::vector> messages; for (size_t i = 0; i < num_msgs_to_publish_; i++) { diff --git a/rosbag2_transport/test/rosbag2_transport/test_play_timing.cpp b/rosbag2_transport/test/rosbag2_transport/test_play_timing.cpp index 9a8271a437..e83e37ae4d 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_play_timing.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_play_timing.cpp @@ -49,7 +49,7 @@ class PlayerTestFixture : public Rosbag2TransportTestFixture auto primitive_message2 = get_messages_strings()[0]; primitive_message2->string_value = "Hello World 2"; - topics_and_types = {{"topic1", "test_msgs/Strings", "", "", ""}}; + topics_and_types = {{"topic1", "test_msgs/Strings", "", {}, ""}}; messages = { serialize_test_message("topic1", 0, primitive_message1), serialize_test_message("topic1", 0, primitive_message2) diff --git a/rosbag2_transport/test/rosbag2_transport/test_play_topic_remap.cpp b/rosbag2_transport/test/rosbag2_transport/test_play_topic_remap.cpp index c1cc64a523..befe982c1a 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_play_topic_remap.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_play_topic_remap.cpp @@ -45,7 +45,7 @@ TEST_F(RosBag2PlayTestFixture, recorded_message_is_played_on_remapped_topic) { primitive_message1->int32_value = test_value; auto topic_types = std::vector{ - {original_topic, "test_msgs/BasicTypes", "", "", ""} + {original_topic, "test_msgs/BasicTypes", "", {}, ""} }; std::vector> messages; diff --git a/rosbag2_transport/test/rosbag2_transport/test_play_until.cpp b/rosbag2_transport/test/rosbag2_transport/test_play_until.cpp index 89aac9e6c4..3ddcd55ba1 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_play_until.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_play_until.cpp @@ -69,7 +69,7 @@ TEST_F(RosBag2PlayUntilTestFixture, play_until_none_are_played_due_to_timestamp) primitive_message2->int32_value = 2; auto topic_types = std::vector{ - {kTopic1Name_, "test_msgs/BasicTypes", "", "", ""}}; + {kTopic1Name_, "test_msgs/BasicTypes", "", {}, ""}}; std::vector> messages = { @@ -112,7 +112,7 @@ TEST_F(RosBag2PlayUntilTestFixture, play_until_less_than_the_total_duration) primitive_message2->int32_value = 2; auto topic_types = std::vector{ - {kTopic1Name_, "test_msgs/BasicTypes", "", "", ""}}; + {kTopic1Name_, "test_msgs/BasicTypes", "", {}, ""}}; std::vector> messages = { @@ -180,7 +180,7 @@ TEST_F( TEST_F(RosBag2PlayUntilTestFixture, play_should_return_false_when_interrupted) { auto topic_types = std::vector{ - {kTopic1Name_, "test_msgs/BasicTypes", "", "", ""}}; + {kTopic1Name_, "test_msgs/BasicTypes", "", {}, ""}}; auto primitive_message = get_messages_basic_types()[0]; primitive_message->int32_value = kIntValue; @@ -268,7 +268,7 @@ TEST_F(RosBag2PlayUntilTestFixture, play_until_is_equal_to_the_total_duration) primitive_message2->int32_value = 2; auto topic_types = std::vector{ - {kTopic1Name_, "test_msgs/BasicTypes", "", "", ""}}; + {kTopic1Name_, "test_msgs/BasicTypes", "", {}, ""}}; std::vector> messages = { diff --git a/rosbag2_transport/test/rosbag2_transport/test_player_stop.cpp b/rosbag2_transport/test/rosbag2_transport/test_player_stop.cpp index 19ba036423..8649cba78d 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_player_stop.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_player_stop.cpp @@ -41,7 +41,7 @@ class Rosbag2PlayerStopTestFixture : public Rosbag2TransportTestFixture { rclcpp::init(0, nullptr); auto topics_and_types = - std::vector{{"topic1", "test_msgs/Strings", "", "", ""}}; + std::vector{{"topic1", "test_msgs/Strings", "", {}, ""}}; std::vector> messages; messages.reserve(num_test_messages_); diff --git a/rosbag2_transport/test/rosbag2_transport/test_record.cpp b/rosbag2_transport/test/rosbag2_transport/test_record.cpp index baff0b1583..48e0d95516 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_record.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_record.cpp @@ -30,7 +30,7 @@ #include "test_msgs/msg/arrays.hpp" #include "test_msgs/message_fixtures.hpp" -#include "rosbag2_transport/qos.hpp" +#include "rosbag2_storage/qos.hpp" #include "record_integration_fixture.hpp" TEST_F(RecordIntegrationTestFixture, published_messages_from_multiple_topics_are_recorded) @@ -175,10 +175,12 @@ TEST_F(RecordIntegrationTestFixture, qos_is_stored_in_metadata) EXPECT_THAT(recorded_messages, SizeIs(expected_messages)); auto recorded_topics = mock_writer.get_topics(); - std::string serialized_profiles = recorded_topics.at(topic).first.offered_qos_profiles; + auto offered_qos_profiles = recorded_topics.at(topic).first.offered_qos_profiles; + std::string serialized_profiles = rosbag2_storage::serialize_rclcpp_qos_vector( + offered_qos_profiles); // Basic smoke test that the profile was serialized into the metadata as a string. - EXPECT_THAT(serialized_profiles, ContainsRegex("reliability: 1\n")); - EXPECT_THAT(serialized_profiles, ContainsRegex("durability: 2\n")); + EXPECT_THAT(serialized_profiles, ContainsRegex("reliability: reliable\n")); + EXPECT_THAT(serialized_profiles, ContainsRegex("durability: volatile\n")); EXPECT_THAT( serialized_profiles, ContainsRegex( "deadline:\n" @@ -191,7 +193,7 @@ TEST_F(RecordIntegrationTestFixture, qos_is_stored_in_metadata) " sec: .+\n" " nsec: .+\n" )); - EXPECT_THAT(serialized_profiles, ContainsRegex("liveliness: 1\n")); + EXPECT_THAT(serialized_profiles, ContainsRegex("liveliness: automatic\n")); EXPECT_THAT( serialized_profiles, ContainsRegex( "liveliness_lease_duration:\n" diff --git a/rosbag2_transport/test/rosbag2_transport/test_rewrite.cpp b/rosbag2_transport/test/rosbag2_transport/test_rewrite.cpp index 3271d12c47..ee5ec074b4 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_rewrite.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_rewrite.cpp @@ -131,9 +131,7 @@ TEST_P(TestRewrite, test_merge) { for (const auto & topic_info : metadata.topics_with_message_count) { const auto topic = topic_info.topic_metadata; if (topic.name == "a_empty") { - YAML::Node qos_node = YAML::Load(topic.offered_qos_profiles); - EXPECT_TRUE(qos_node.IsSequence()); - EXPECT_EQ(qos_node.size(), 3u); + EXPECT_EQ(topic.offered_qos_profiles.size(), 3u); } } }