From f34e165c6fa389ff1eaf816b04c13a8e9379e942 Mon Sep 17 00:00:00 2001 From: Patrick Roncagliolo Date: Tue, 3 Oct 2023 10:29:40 +0200 Subject: [PATCH 01/51] Make C++ QoS YAML (de)serialization compliant with QoS override file schema Signed-off-by: Patrick Roncagliolo Signed-off-by: roncapat --- .../include/rosbag2_transport/qos.hpp | 28 +++++++ .../src/rosbag2_transport/qos.cpp | 74 +++++++++++++++---- .../test/rosbag2_transport/test_qos.cpp | 16 ++-- 3 files changed, 97 insertions(+), 21 deletions(-) diff --git a/rosbag2_transport/include/rosbag2_transport/qos.hpp b/rosbag2_transport/include/rosbag2_transport/qos.hpp index 9268bc4117..8c441d1592 100644 --- a/rosbag2_transport/include/rosbag2_transport/qos.hpp +++ b/rosbag2_transport/include/rosbag2_transport/qos.hpp @@ -70,6 +70,34 @@ class ROSBAG2_TRANSPORT_PUBLIC Rosbag2QoS : public rclcpp::QoS namespace YAML { +template<> +struct ROSBAG2_TRANSPORT_PUBLIC convert +{ + static Node encode(const rmw_qos_history_policy_t & policy); + static bool decode(const Node & node, rmw_qos_history_policy_t & policy); +}; + +template<> +struct ROSBAG2_TRANSPORT_PUBLIC convert +{ + static Node encode(const rmw_qos_reliability_policy_t & policy); + static bool decode(const Node & node, rmw_qos_reliability_policy_t & policy); +}; + +template<> +struct ROSBAG2_TRANSPORT_PUBLIC convert +{ + static Node encode(const rmw_qos_durability_policy_t & policy); + static bool decode(const Node & node, rmw_qos_durability_policy_t & policy); +}; + +template<> +struct ROSBAG2_TRANSPORT_PUBLIC convert +{ + static Node encode(const rmw_qos_liveliness_policy_t & policy); + static bool decode(const Node & node, rmw_qos_liveliness_policy_t & policy); +}; + template<> struct ROSBAG2_TRANSPORT_PUBLIC convert { diff --git a/rosbag2_transport/src/rosbag2_transport/qos.cpp b/rosbag2_transport/src/rosbag2_transport/qos.cpp index f643274a86..9df0148243 100644 --- a/rosbag2_transport/src/rosbag2_transport/qos.cpp +++ b/rosbag2_transport/src/rosbag2_transport/qos.cpp @@ -18,6 +18,7 @@ #include "rosbag2_transport/qos.hpp" #include "logging.hpp" +#include "rmw/qos_string_conversions.h" namespace { @@ -39,8 +40,60 @@ 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) +{ + 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) +{ + 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) +{ + 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) +{ + 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; @@ -67,13 +120,13 @@ Node convert::encode(const rosbag2_transport::Ros { const auto & p = qos.get_rmw_qos_profile(); Node node; - node["history"] = static_cast(p.history); + node["history"] = convert::encode(p.history); node["depth"] = p.depth; - node["reliability"] = static_cast(p.reliability); - node["durability"] = static_cast(p.durability); + node["reliability"] = convert::encode(p.reliability); + node["durability"] = convert::encode(p.durability); node["deadline"] = p.deadline; node["lifespan"] = p.lifespan; - node["liveliness"] = static_cast(p.liveliness); + node["liveliness"] = convert::encode(p.liveliness); node["liveliness_lease_duration"] = p.liveliness_lease_duration; node["avoid_ros_namespace_conventions"] = p.avoid_ros_namespace_conventions; return node; @@ -82,19 +135,14 @@ Node convert::encode(const rosbag2_transport::Ros 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) + .history(node["history"].as()) + .reliability(node["reliability"].as()) + .durability(node["durability"].as()) .deadline(node["deadline"].as()) .lifespan(node["lifespan"].as()) - .liveliness(liveliness) + .liveliness(node["liveliness"].as()) .liveliness_lease_duration(node["liveliness_lease_duration"].as()) .avoid_ros_namespace_conventions(node["avoid_ros_namespace_conventions"].as()); return true; diff --git a/rosbag2_transport/test/rosbag2_transport/test_qos.cpp b/rosbag2_transport/test/rosbag2_transport/test_qos.cpp index 3c715d6cb3..c87779b782 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_qos.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_qos.cpp @@ -41,17 +41,17 @@ TEST(TestQoS, supports_version_4) // This test shows how this data looks at the time of introduction // Bags created by this version of rosbag2 look like this and must be able to be read std::string serialized_profiles = - "- 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: 0\n" " nsec: 0\n" " lifespan:\n" " sec: 0\n" " nsec: 0\n" - " liveliness: 0\n" + " liveliness: system_default\n" " liveliness_lease_duration:\n" " sec: 0\n" " nsec: 0\n" @@ -118,17 +118,17 @@ 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" From 30832e89cf93f271a5d5b3c6a865cd758a6f8850 Mon Sep 17 00:00:00 2001 From: roncapat Date: Sat, 7 Oct 2023 15:27:12 +0200 Subject: [PATCH 02/51] UGLY DRAFT - handle different versions of TopicMetadata Signed-off-by: Patrick Roncagliolo --- .../include/rosbag2_storage/bag_metadata.hpp | 2 +- .../rosbag2_storage/topic_metadata.hpp | 1 + .../include/rosbag2_storage/yaml.hpp | 4 +- .../test_metadata_serialization.cpp | 10 +-- .../include/rosbag2_transport/qos.hpp | 43 ++++++++++- .../src/rosbag2_transport/player.cpp | 9 ++- .../src/rosbag2_transport/qos.cpp | 73 ++++++++++++++++++- .../src/rosbag2_transport/record_options.cpp | 8 +- .../src/rosbag2_transport/recorder.cpp | 2 +- .../test/rosbag2_transport/test_play.cpp | 2 +- .../test/rosbag2_transport/test_qos.cpp | 16 ++-- 11 files changed, 140 insertions(+), 30 deletions(-) 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/topic_metadata.hpp b/rosbag2_storage/include/rosbag2_storage/topic_metadata.hpp index cb8339f238..59625a6238 100644 --- a/rosbag2_storage/include/rosbag2_storage/topic_metadata.hpp +++ b/rosbag2_storage/include/rosbag2_storage/topic_metadata.hpp @@ -29,6 +29,7 @@ struct TopicMetadata std::string offered_qos_profiles; // REP-2011 type description hash if available for topic, "" otherwise. std::string type_description_hash; + int version = 9; bool operator==(const rosbag2_storage::TopicMetadata & rhs) const { diff --git a/rosbag2_storage/include/rosbag2_storage/yaml.hpp b/rosbag2_storage/include/rosbag2_storage/yaml.hpp index 601c9a2dbe..7e6d536a86 100644 --- a/rosbag2_storage/include/rosbag2_storage/yaml.hpp +++ b/rosbag2_storage/include/rosbag2_storage/yaml.hpp @@ -102,6 +102,7 @@ struct convert static bool decode(const Node & node, rosbag2_storage::TopicMetadata & topic, int version) { + topic.version = version; topic.name = node["name"].as(); topic.type = node["type"].as(); topic.serialization_format = node["serialization_format"].as(); @@ -160,7 +161,8 @@ struct convert> rhs.clear(); for (const auto & value : node) { - rhs.push_back(decode_for_version(value, version)); + auto temp = decode_for_version(value, version); + rhs.push_back(temp); } return true; } diff --git a/rosbag2_storage/test/rosbag2_storage/test_metadata_serialization.cpp b/rosbag2_storage/test/rosbag2_storage/test_metadata_serialization.cpp index 084aa0d2c1..d824b35c91 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", "qos1", "", 8}, 100}); + metadata.topics_with_message_count.push_back({{"topic2", "type2", "rmw2", "qos2", "", 8}, 200}); metadata_io_->write_metadata(temporary_dir_path_, metadata); auto read_metadata = metadata_io_->read_metadata(temporary_dir_path_); @@ -105,7 +105,7 @@ TEST_F(MetadataFixture, metadata_reads_v3_check_offered_qos_profiles_empty) BagMetadata metadata{}; metadata.version = 3; metadata.topics_with_message_count.push_back( - {{"topic", "type", "rmw", offered_qos_profiles, ""}, message_count}); + {{"topic", "type", "rmw", offered_qos_profiles, "", 3}, message_count}); metadata_io_->write_metadata(temporary_dir_path_, metadata); auto read_metadata = metadata_io_->read_metadata(temporary_dir_path_); ASSERT_THAT( @@ -124,7 +124,7 @@ TEST_F(MetadataFixture, metadata_reads_v4_fills_offered_qos_profiles) BagMetadata metadata{}; metadata.version = 4; metadata.topics_with_message_count.push_back( - {{"topic", "type", "rmw", offered_qos_profiles, ""}, message_count}); + {{"topic", "type", "rmw", offered_qos_profiles, "", 4}, message_count}); metadata_io_->write_metadata(temporary_dir_path_, metadata); auto read_metadata = metadata_io_->read_metadata(temporary_dir_path_); ASSERT_THAT( @@ -154,7 +154,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", "", type_description_hash, 7}, 1}); metadata_io_->write_metadata(temporary_dir_path_, metadata); auto read_metadata = metadata_io_->read_metadata(temporary_dir_path_); diff --git a/rosbag2_transport/include/rosbag2_transport/qos.hpp b/rosbag2_transport/include/rosbag2_transport/qos.hpp index 8c441d1592..14f9b4cb4f 100644 --- a/rosbag2_transport/include/rosbag2_transport/qos.hpp +++ b/rosbag2_transport/include/rosbag2_transport/qos.hpp @@ -66,6 +66,36 @@ class ROSBAG2_TRANSPORT_PUBLIC Rosbag2QoS : public rclcpp::QoS const std::string & topic_name, const std::vector & profiles); }; + +template +class ROSBAG2_TRANSPORT_PUBLIC Rosbag2QoS_v : public rosbag2_transport::Rosbag2QoS +{ +public: + Rosbag2QoS_v() + : rosbag2_transport::Rosbag2QoS() {} + + explicit Rosbag2QoS_v(const rclcpp::QoS & value) + : rosbag2_transport::Rosbag2QoS(value) {} + + explicit Rosbag2QoS_v(const Rosbag2QoS & value) + : rosbag2_transport::Rosbag2QoS(value) {} + + Rosbag2QoS_v & default_history() + { + keep_last(rmw_qos_profile_default.depth); + return *this; + } + + static Rosbag2QoS_v adapt_request_to_offers( + const std::string & topic_name, + const std::vector & endpoints){ + return rosbag2_transport::Rosbag2QoS::adapt_request_to_offers(topic_name, endpoints); + }; + + static Rosbag2QoS_v adapt_offer_to_recorded_offers( + const std::string & topic_name, + const std::vector> & profiles); +}; } // namespace rosbag2_transport namespace YAML @@ -106,10 +136,17 @@ struct ROSBAG2_TRANSPORT_PUBLIC convert }; template<> -struct ROSBAG2_TRANSPORT_PUBLIC convert +struct ROSBAG2_TRANSPORT_PUBLIC convert> +{ + static Node encode(const rosbag2_transport::Rosbag2QoS_v<8> & qos); + static bool decode(const Node & node, rosbag2_transport::Rosbag2QoS_v<8> & qos); +}; + +template<> +struct ROSBAG2_TRANSPORT_PUBLIC convert> { - static Node encode(const rosbag2_transport::Rosbag2QoS & qos); - static bool decode(const Node & node, rosbag2_transport::Rosbag2QoS & qos); + static Node encode(const rosbag2_transport::Rosbag2QoS_v<9> & qos); + static bool decode(const Node & node, rosbag2_transport::Rosbag2QoS_v<9> & qos); }; } // namespace YAML diff --git a/rosbag2_transport/src/rosbag2_transport/player.cpp b/rosbag2_transport/src/rosbag2_transport/player.cpp index 98c767e24e..0987c55480 100644 --- a/rosbag2_transport/src/rosbag2_transport/player.cpp +++ b/rosbag2_transport/src/rosbag2_transport/player.cpp @@ -80,8 +80,13 @@ rclcpp::QoS publisher_qos_for_topic( } 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); + if (topic.version <=8){ + const auto offered_qos_profiles = profiles_yaml.as>>(); + return rosbag2_transport::Rosbag2QoS_v<8>::adapt_offer_to_recorded_offers(topic.name, offered_qos_profiles); + } else { + const auto offered_qos_profiles = profiles_yaml.as>>(); + return rosbag2_transport::Rosbag2QoS_v<9>::adapt_offer_to_recorded_offers(topic.name, offered_qos_profiles); + } } } // namespace diff --git a/rosbag2_transport/src/rosbag2_transport/qos.cpp b/rosbag2_transport/src/rosbag2_transport/qos.cpp index 9df0148243..aa4265591b 100644 --- a/rosbag2_transport/src/rosbag2_transport/qos.cpp +++ b/rosbag2_transport/src/rosbag2_transport/qos.cpp @@ -116,7 +116,44 @@ bool convert::decode(const Node & node, rmw_time_t & time) return true; } -Node convert::encode(const rosbag2_transport::Rosbag2QoS & qos) +Node convert>::encode(const rosbag2_transport::Rosbag2QoS_v<8> & 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_v<8> & 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; +} + +Node convert>::encode(const rosbag2_transport::Rosbag2QoS_v<9> & qos) { const auto & p = qos.get_rmw_qos_profile(); Node node; @@ -132,8 +169,8 @@ Node convert::encode(const rosbag2_transport::Ros return node; } -bool convert::decode( - const Node & node, rosbag2_transport::Rosbag2QoS & qos) +bool convert>::decode( + const Node & node, rosbag2_transport::Rosbag2QoS_v<9> & qos) { qos .keep_last(node["depth"].as()) @@ -231,7 +268,8 @@ bool operator==(const rmw_time_t & lhs, const rmw_time_t & rhs) * policies that affect compatibility. * This means it excludes history and lifespan from the equality check. */ -bool all_profiles_effectively_same(const std::vector & profiles) +template +bool all_profiles_effectively_same(const std::vector & profiles) { auto iterator = profiles.begin(); const auto p_ref = iterator->get_rmw_qos_profile(); @@ -272,4 +310,31 @@ Rosbag2QoS Rosbag2QoS::adapt_offer_to_recorded_offers( "Falling back to the rosbag2_transport default publisher offer."); return Rosbag2QoS{}; } + +template +Rosbag2QoS_v Rosbag2QoS_v::adapt_offer_to_recorded_offers( + const std::string & topic_name, const std::vector> & profiles) +{ + if (profiles.empty()) { + return Rosbag2QoS_v{}; + } + 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_v{}; +} + +template +Rosbag2QoS_v<8> Rosbag2QoS_v<8>::adapt_offer_to_recorded_offers( + const std::string & topic_name, const std::vector> & profiles); + +template +Rosbag2QoS_v<9> Rosbag2QoS_v<9>::adapt_offer_to_recorded_offers( + const std::string & topic_name, const std::vector> & profiles); } // 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..f462ca74d2 100644 --- a/rosbag2_transport/src/rosbag2_transport/record_options.cpp +++ b/rosbag2_transport/src/rosbag2_transport/record_options.cpp @@ -54,10 +54,10 @@ 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; @@ -82,8 +82,8 @@ 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>( + std::map> qos_overrides; + optional_assign>>( node, "topic_qos_profile_overrides", qos_overrides); record_options.topic_qos_profile_overrides.insert(qos_overrides.begin(), qos_overrides.end()); diff --git a/rosbag2_transport/src/rosbag2_transport/recorder.cpp b/rosbag2_transport/src/rosbag2_transport/recorder.cpp index 69c280e136..3edc86ee82 100644 --- a/rosbag2_transport/src/rosbag2_transport/recorder.cpp +++ b/rosbag2_transport/src/rosbag2_transport/recorder.cpp @@ -508,7 +508,7 @@ std::string RecorderImpl::serialized_offered_qos_profiles_for_topic( { YAML::Node offered_qos_profiles; for (const auto & info : topics_endpoint_info) { - offered_qos_profiles.push_back(Rosbag2QoS(info.qos_profile())); + offered_qos_profiles.push_back(Rosbag2QoS_v<>(info.qos_profile())); } return YAML::Dump(offered_qos_profiles); } diff --git a/rosbag2_transport/test/rosbag2_transport/test_play.cpp b/rosbag2_transport/test/rosbag2_transport/test_play.cpp index 7b33abbcdc..9861d098d0 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_play.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_play.cpp @@ -471,7 +471,7 @@ class RosBag2PlayQosOverrideTestFixture : public RosBag2PlayTestFixture if (!offered_qos.empty()) { YAML::Node offered_qos_yaml; for (const auto & profile : offered_qos) { - offered_qos_yaml.push_back(profile); + offered_qos_yaml.push_back>(static_cast>(profile)); } serialized_offered_qos = YAML::Dump(offered_qos_yaml); } diff --git a/rosbag2_transport/test/rosbag2_transport/test_qos.cpp b/rosbag2_transport/test/rosbag2_transport/test_qos.cpp index c87779b782..6de3b2c462 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_qos.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_qos.cpp @@ -23,13 +23,13 @@ TEST(TestQoS, serialization) { - rosbag2_transport::Rosbag2QoS expected_qos; + rosbag2_transport::Rosbag2QoS_v<> 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]; @@ -41,24 +41,24 @@ TEST(TestQoS, supports_version_4) // This test shows how this data looks at the time of introduction // Bags created by this version of rosbag2 look like this and must be able to be read std::string serialized_profiles = - "- history: system_default\n" + "- history: 1\n" " depth: 10\n" - " reliability: reliable\n" - " durability: volatile\n" + " reliability: 1\n" + " durability: 2\n" " deadline:\n" " sec: 0\n" " nsec: 0\n" " lifespan:\n" " sec: 0\n" " nsec: 0\n" - " liveliness: system_default\n" + " liveliness: 0\n" " liveliness_lease_duration:\n" " sec: 0\n" " nsec: 0\n" " avoid_ros_namespace_conventions: false\n"; YAML::Node loaded_node = YAML::Load(serialized_profiles); - auto deserialized_profiles = loaded_node.as>(); + auto deserialized_profiles = loaded_node.as>>(); ASSERT_EQ(deserialized_profiles.size(), 1u); auto actual_qos = deserialized_profiles[0].get_rmw_qos_profile(); @@ -134,7 +134,7 @@ TEST(TestQoS, translates_bad_infinity_values) " 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)); From 8d704fed4e1ea72469731eadc1547148082bea97 Mon Sep 17 00:00:00 2001 From: roncapat Date: Sat, 7 Oct 2023 15:55:47 +0200 Subject: [PATCH 03/51] Handle unknown cases Signed-off-by: Patrick Roncagliolo --- .../src/rosbag2_transport/qos.cpp | 25 +++++++++++++++---- 1 file changed, 20 insertions(+), 5 deletions(-) diff --git a/rosbag2_transport/src/rosbag2_transport/qos.cpp b/rosbag2_transport/src/rosbag2_transport/qos.cpp index aa4265591b..0109377554 100644 --- a/rosbag2_transport/src/rosbag2_transport/qos.cpp +++ b/rosbag2_transport/src/rosbag2_transport/qos.cpp @@ -46,7 +46,11 @@ namespace YAML Node convert::encode(const rmw_qos_history_policy_t & policy) { - return Node(std::string(rmw_qos_history_policy_to_str(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) @@ -57,7 +61,11 @@ bool convert::decode(const Node & node, rmw_qos_histor Node convert::encode(const rmw_qos_reliability_policy_t & policy) { - return Node(std::string(rmw_qos_reliability_policy_to_str(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( @@ -70,8 +78,11 @@ bool convert::decode( Node convert::encode(const rmw_qos_durability_policy_t & policy) { - return Node(std::string(rmw_qos_durability_policy_to_str(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, @@ -83,7 +94,11 @@ bool convert::decode( Node convert::encode(const rmw_qos_liveliness_policy_t & policy) { - return Node(std::string(rmw_qos_liveliness_policy_to_str(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( From d0434e3d29e5f7bd418549462e648edcb9b03696 Mon Sep 17 00:00:00 2001 From: roncapat Date: Sat, 7 Oct 2023 16:13:32 +0200 Subject: [PATCH 04/51] Uncrustify Signed-off-by: Patrick Roncagliolo --- .../include/rosbag2_transport/qos.hpp | 9 ++++---- .../src/rosbag2_transport/player.cpp | 16 +++++++++----- .../src/rosbag2_transport/qos.cpp | 21 +++++++++++-------- .../src/rosbag2_transport/record_options.cpp | 3 ++- .../test/rosbag2_transport/test_play.cpp | 4 +++- 5 files changed, 33 insertions(+), 20 deletions(-) diff --git a/rosbag2_transport/include/rosbag2_transport/qos.hpp b/rosbag2_transport/include/rosbag2_transport/qos.hpp index 14f9b4cb4f..4b6a31d9e8 100644 --- a/rosbag2_transport/include/rosbag2_transport/qos.hpp +++ b/rosbag2_transport/include/rosbag2_transport/qos.hpp @@ -67,7 +67,7 @@ class ROSBAG2_TRANSPORT_PUBLIC Rosbag2QoS : public rclcpp::QoS const std::vector & profiles); }; -template +template class ROSBAG2_TRANSPORT_PUBLIC Rosbag2QoS_v : public rosbag2_transport::Rosbag2QoS { public: @@ -88,9 +88,10 @@ class ROSBAG2_TRANSPORT_PUBLIC Rosbag2QoS_v : public rosbag2_transport::Rosbag2Q static Rosbag2QoS_v adapt_request_to_offers( const std::string & topic_name, - const std::vector & endpoints){ - return rosbag2_transport::Rosbag2QoS::adapt_request_to_offers(topic_name, endpoints); - }; + const std::vector & endpoints) + { + return rosbag2_transport::Rosbag2QoS::adapt_request_to_offers(topic_name, endpoints); + } static Rosbag2QoS_v adapt_offer_to_recorded_offers( const std::string & topic_name, diff --git a/rosbag2_transport/src/rosbag2_transport/player.cpp b/rosbag2_transport/src/rosbag2_transport/player.cpp index 0987c55480..58ba5f1569 100644 --- a/rosbag2_transport/src/rosbag2_transport/player.cpp +++ b/rosbag2_transport/src/rosbag2_transport/player.cpp @@ -80,12 +80,18 @@ rclcpp::QoS publisher_qos_for_topic( } const auto profiles_yaml = YAML::Load(topic.offered_qos_profiles); - if (topic.version <=8){ - const auto offered_qos_profiles = profiles_yaml.as>>(); - return rosbag2_transport::Rosbag2QoS_v<8>::adapt_offer_to_recorded_offers(topic.name, offered_qos_profiles); + if (topic.version <= 8) { + const auto offered_qos_profiles = + profiles_yaml.as>>(); + return rosbag2_transport::Rosbag2QoS_v<8>::adapt_offer_to_recorded_offers( + topic.name, + offered_qos_profiles); } else { - const auto offered_qos_profiles = profiles_yaml.as>>(); - return rosbag2_transport::Rosbag2QoS_v<9>::adapt_offer_to_recorded_offers(topic.name, offered_qos_profiles); + const auto offered_qos_profiles = + profiles_yaml.as>>(); + return rosbag2_transport::Rosbag2QoS_v<9>::adapt_offer_to_recorded_offers( + topic.name, + offered_qos_profiles); } } } // namespace diff --git a/rosbag2_transport/src/rosbag2_transport/qos.cpp b/rosbag2_transport/src/rosbag2_transport/qos.cpp index 0109377554..078d2d2631 100644 --- a/rosbag2_transport/src/rosbag2_transport/qos.cpp +++ b/rosbag2_transport/src/rosbag2_transport/qos.cpp @@ -46,7 +46,7 @@ namespace YAML Node convert::encode(const rmw_qos_history_policy_t & policy) { - if (policy == RMW_QOS_POLICY_HISTORY_UNKNOWN){ + if (policy == RMW_QOS_POLICY_HISTORY_UNKNOWN) { return Node(std::string("unknown")); } else { return Node(std::string(rmw_qos_history_policy_to_str(policy))); @@ -61,7 +61,7 @@ bool convert::decode(const Node & node, rmw_qos_histor Node convert::encode(const rmw_qos_reliability_policy_t & policy) { - if (policy == RMW_QOS_POLICY_RELIABILITY_UNKNOWN){ + if (policy == RMW_QOS_POLICY_RELIABILITY_UNKNOWN) { return Node(std::string("unknown")); } else { return Node(std::string(rmw_qos_reliability_policy_to_str(policy))); @@ -78,11 +78,12 @@ bool convert::decode( Node convert::encode(const rmw_qos_durability_policy_t & policy) { - if (policy == RMW_QOS_POLICY_DURABILITY_UNKNOWN){ + 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, @@ -94,7 +95,7 @@ bool convert::decode( Node convert::encode(const rmw_qos_liveliness_policy_t & policy) { - if (policy == RMW_QOS_POLICY_LIVELINESS_UNKNOWN){ + if (policy == RMW_QOS_POLICY_LIVELINESS_UNKNOWN) { return Node(std::string("unknown")); } else { return Node(std::string(rmw_qos_liveliness_policy_to_str(policy))); @@ -131,7 +132,8 @@ bool convert::decode(const Node & node, rmw_time_t & time) return true; } -Node convert>::encode(const rosbag2_transport::Rosbag2QoS_v<8> & qos) +Node convert>::encode( + const rosbag2_transport::Rosbag2QoS_v<8> & qos) { const auto & p = qos.get_rmw_qos_profile(); Node node; @@ -168,7 +170,8 @@ bool convert>::decode( return true; } -Node convert>::encode(const rosbag2_transport::Rosbag2QoS_v<9> & qos) +Node convert>::encode( + const rosbag2_transport::Rosbag2QoS_v<9> & qos) { const auto & p = qos.get_rmw_qos_profile(); Node node; @@ -283,7 +286,7 @@ bool operator==(const rmw_time_t & lhs, const rmw_time_t & rhs) * policies that affect compatibility. * This means it excludes history and lifespan from the equality check. */ -template +template bool all_profiles_effectively_same(const std::vector & profiles) { auto iterator = profiles.begin(); @@ -348,7 +351,7 @@ Rosbag2QoS_v Rosbag2QoS_v::adapt_offer_to_recorded_offers( template Rosbag2QoS_v<8> Rosbag2QoS_v<8>::adapt_offer_to_recorded_offers( const std::string & topic_name, const std::vector> & profiles); - + template Rosbag2QoS_v<9> Rosbag2QoS_v<9>::adapt_offer_to_recorded_offers( const std::string & topic_name, const std::vector> & profiles); diff --git a/rosbag2_transport/src/rosbag2_transport/record_options.cpp b/rosbag2_transport/src/rosbag2_transport/record_options.cpp index f462ca74d2..6aa5a4dc4b 100644 --- a/rosbag2_transport/src/rosbag2_transport/record_options.cpp +++ b/rosbag2_transport/src/rosbag2_transport/record_options.cpp @@ -57,7 +57,8 @@ Node convert::encode( std::map> qos_overrides( record_options.topic_qos_profile_overrides.begin(), record_options.topic_qos_profile_overrides.end()); - node["topic_qos_profile_overrides"] = convert>>::encode(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; diff --git a/rosbag2_transport/test/rosbag2_transport/test_play.cpp b/rosbag2_transport/test/rosbag2_transport/test_play.cpp index 9861d098d0..e61ca4aa46 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_play.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_play.cpp @@ -471,7 +471,9 @@ class RosBag2PlayQosOverrideTestFixture : public RosBag2PlayTestFixture if (!offered_qos.empty()) { YAML::Node offered_qos_yaml; for (const auto & profile : offered_qos) { - offered_qos_yaml.push_back>(static_cast>(profile)); + offered_qos_yaml.push_back>( + static_cast>(profile)); } serialized_offered_qos = YAML::Dump(offered_qos_yaml); } From 545ec1c8780ffc22976bc12d0a80c4165500d732 Mon Sep 17 00:00:00 2001 From: roncapat Date: Sat, 7 Oct 2023 16:16:59 +0200 Subject: [PATCH 05/51] Fix test Signed-off-by: Patrick Roncagliolo --- rosbag2_transport/test/rosbag2_transport/test_record.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/rosbag2_transport/test/rosbag2_transport/test_record.cpp b/rosbag2_transport/test/rosbag2_transport/test_record.cpp index baff0b1583..266bba90a8 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_record.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_record.cpp @@ -177,8 +177,8 @@ TEST_F(RecordIntegrationTestFixture, qos_is_stored_in_metadata) auto recorded_topics = mock_writer.get_topics(); std::string serialized_profiles = recorded_topics.at(topic).first.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 +191,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" From ec87bcd5c4df81972a9a482fa0693c4dc3d2b5c1 Mon Sep 17 00:00:00 2001 From: roncapat Date: Sat, 7 Oct 2023 17:31:16 +0200 Subject: [PATCH 06/51] Refactor to avoid templates Signed-off-by: Patrick Roncagliolo --- .../include/rosbag2_transport/qos.hpp | 92 ++++++++++-------- .../rosbag2_transport/record_options.hpp | 4 +- .../src/rosbag2_transport/player.cpp | 20 ++-- .../src/rosbag2_transport/qos.cpp | 93 +++++-------------- .../src/rosbag2_transport/record_options.cpp | 14 +-- .../src/rosbag2_transport/recorder.cpp | 2 +- .../test/rosbag2_transport/test_play.cpp | 4 +- .../test/rosbag2_transport/test_qos.cpp | 11 ++- .../rosbag2_transport/test_record_options.cpp | 3 +- 9 files changed, 105 insertions(+), 138 deletions(-) diff --git a/rosbag2_transport/include/rosbag2_transport/qos.hpp b/rosbag2_transport/include/rosbag2_transport/qos.hpp index 4b6a31d9e8..dfdd1ad020 100644 --- a/rosbag2_transport/include/rosbag2_transport/qos.hpp +++ b/rosbag2_transport/include/rosbag2_transport/qos.hpp @@ -66,37 +66,6 @@ class ROSBAG2_TRANSPORT_PUBLIC Rosbag2QoS : public rclcpp::QoS const std::string & topic_name, const std::vector & profiles); }; - -template -class ROSBAG2_TRANSPORT_PUBLIC Rosbag2QoS_v : public rosbag2_transport::Rosbag2QoS -{ -public: - Rosbag2QoS_v() - : rosbag2_transport::Rosbag2QoS() {} - - explicit Rosbag2QoS_v(const rclcpp::QoS & value) - : rosbag2_transport::Rosbag2QoS(value) {} - - explicit Rosbag2QoS_v(const Rosbag2QoS & value) - : rosbag2_transport::Rosbag2QoS(value) {} - - Rosbag2QoS_v & default_history() - { - keep_last(rmw_qos_profile_default.depth); - return *this; - } - - static Rosbag2QoS_v adapt_request_to_offers( - const std::string & topic_name, - const std::vector & endpoints) - { - return rosbag2_transport::Rosbag2QoS::adapt_request_to_offers(topic_name, endpoints); - } - - static Rosbag2QoS_v adapt_offer_to_recorded_offers( - const std::string & topic_name, - const std::vector> & profiles); -}; } // namespace rosbag2_transport namespace YAML @@ -137,17 +106,66 @@ struct ROSBAG2_TRANSPORT_PUBLIC convert }; template<> -struct ROSBAG2_TRANSPORT_PUBLIC convert> +struct ROSBAG2_TRANSPORT_PUBLIC convert { - static Node encode(const rosbag2_transport::Rosbag2QoS_v<8> & qos); - static bool decode(const Node & node, rosbag2_transport::Rosbag2QoS_v<8> & qos); + static Node encode(const rosbag2_transport::Rosbag2QoS & qos); + static bool decode(const Node & node, rosbag2_transport::Rosbag2QoS & qos, int version); }; template<> -struct ROSBAG2_TRANSPORT_PUBLIC convert> +struct ROSBAG2_TRANSPORT_PUBLIC convert> { - static Node encode(const rosbag2_transport::Rosbag2QoS_v<9> & qos); - static bool decode(const Node & node, rosbag2_transport::Rosbag2QoS_v<9> & qos); + static Node encode(const std::vector & rhs) + { + Node node{NodeType::Sequence}; + for (const auto & value : rhs) { + node.push_back(value); + } + return node; + } + + static bool decode( + const Node & node, std::vector & rhs, int version) + { + if (!node.IsSequence()) { + return false; + } + + rhs.clear(); + for (const auto & value : node) { + auto temp = decode_for_version(value, version); + rhs.push_back(temp); + } + return true; + } +}; + +template<> +struct ROSBAG2_TRANSPORT_PUBLIC convert> +{ + static Node encode(const std::map & rhs) + { + Node node{NodeType::Sequence}; + for (const auto & [key, value] : rhs) { + node.force_insert(key, value); + } + return node; + } + + static bool decode( + const Node & node, std::map & rhs, int version) + { + if (!node.IsMap()) { + return false; + } + + rhs.clear(); + for (const auto & element : node) { + auto temp = decode_for_version(element.second, version); + rhs[element.first.as()] = temp; + } + return true; + } }; } // namespace YAML diff --git a/rosbag2_transport/include/rosbag2_transport/record_options.hpp b/rosbag2_transport/include/rosbag2_transport/record_options.hpp index a387ab0998..d67ba45cf4 100644 --- a/rosbag2_transport/include/rosbag2_transport/record_options.hpp +++ b/rosbag2_transport/include/rosbag2_transport/record_options.hpp @@ -58,7 +58,9 @@ 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); }; } // namespace YAML diff --git a/rosbag2_transport/src/rosbag2_transport/player.cpp b/rosbag2_transport/src/rosbag2_transport/player.cpp index 58ba5f1569..367c250cf4 100644 --- a/rosbag2_transport/src/rosbag2_transport/player.cpp +++ b/rosbag2_transport/src/rosbag2_transport/player.cpp @@ -80,19 +80,13 @@ rclcpp::QoS publisher_qos_for_topic( } const auto profiles_yaml = YAML::Load(topic.offered_qos_profiles); - if (topic.version <= 8) { - const auto offered_qos_profiles = - profiles_yaml.as>>(); - return rosbag2_transport::Rosbag2QoS_v<8>::adapt_offer_to_recorded_offers( - topic.name, - offered_qos_profiles); - } else { - const auto offered_qos_profiles = - profiles_yaml.as>>(); - return rosbag2_transport::Rosbag2QoS_v<9>::adapt_offer_to_recorded_offers( - topic.name, - offered_qos_profiles); - } + const auto offered_qos_profiles = + YAML::decode_for_version>( + profiles_yaml, + topic.version); + return rosbag2_transport::Rosbag2QoS::adapt_offer_to_recorded_offers( + topic.name, + offered_qos_profiles); } } // namespace diff --git a/rosbag2_transport/src/rosbag2_transport/qos.cpp b/rosbag2_transport/src/rosbag2_transport/qos.cpp index 078d2d2631..0d1bcb0ecf 100644 --- a/rosbag2_transport/src/rosbag2_transport/qos.cpp +++ b/rosbag2_transport/src/rosbag2_transport/qos.cpp @@ -132,46 +132,38 @@ bool convert::decode(const Node & node, rmw_time_t & time) return true; } -Node convert>::encode( - const rosbag2_transport::Rosbag2QoS_v<8> & qos) +bool convert::decode( + const Node & node, rosbag2_transport::Rosbag2QoS & qos, int version) { - 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_v<8> & 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()); + if (version <= 8) { + 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 + .history(history) + .reliability(reliability) + .durability(durability) + .liveliness(liveliness); + } else { + qos + .history(node["history"].as()) + .reliability(node["reliability"].as()) + .durability(node["durability"].as()) + .liveliness(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; } -Node convert>::encode( - const rosbag2_transport::Rosbag2QoS_v<9> & qos) +Node convert::encode( + const rosbag2_transport::Rosbag2QoS & qos) { const auto & p = qos.get_rmw_qos_profile(); Node node; @@ -186,22 +178,6 @@ Node convert>::encode( node["avoid_ros_namespace_conventions"] = p.avoid_ros_namespace_conventions; return node; } - -bool convert>::decode( - const Node & node, rosbag2_transport::Rosbag2QoS_v<9> & qos) -{ - qos - .keep_last(node["depth"].as()) - .history(node["history"].as()) - .reliability(node["reliability"].as()) - .durability(node["durability"].as()) - .deadline(node["deadline"].as()) - .lifespan(node["lifespan"].as()) - .liveliness(node["liveliness"].as()) - .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 @@ -328,31 +304,4 @@ Rosbag2QoS Rosbag2QoS::adapt_offer_to_recorded_offers( "Falling back to the rosbag2_transport default publisher offer."); return Rosbag2QoS{}; } - -template -Rosbag2QoS_v Rosbag2QoS_v::adapt_offer_to_recorded_offers( - const std::string & topic_name, const std::vector> & profiles) -{ - if (profiles.empty()) { - return Rosbag2QoS_v{}; - } - 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_v{}; -} - -template -Rosbag2QoS_v<8> Rosbag2QoS_v<8>::adapt_offer_to_recorded_offers( - const std::string & topic_name, const std::vector> & profiles); - -template -Rosbag2QoS_v<9> Rosbag2QoS_v<9>::adapt_offer_to_recorded_offers( - const std::string & topic_name, const std::vector> & profiles); } // namespace rosbag2_transport diff --git a/rosbag2_transport/src/rosbag2_transport/record_options.cpp b/rosbag2_transport/src/rosbag2_transport/record_options.cpp index 6aa5a4dc4b..d9ccfeb330 100644 --- a/rosbag2_transport/src/rosbag2_transport/record_options.cpp +++ b/rosbag2_transport/src/rosbag2_transport/record_options.cpp @@ -54,18 +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"] = convert>>::encode(qos_overrides); + rosbag2_transport::Rosbag2QoS>>::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); @@ -83,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 3edc86ee82..af3cca8fd0 100644 --- a/rosbag2_transport/src/rosbag2_transport/recorder.cpp +++ b/rosbag2_transport/src/rosbag2_transport/recorder.cpp @@ -508,7 +508,7 @@ std::string RecorderImpl::serialized_offered_qos_profiles_for_topic( { YAML::Node offered_qos_profiles; for (const auto & info : topics_endpoint_info) { - offered_qos_profiles.push_back(Rosbag2QoS_v<>(info.qos_profile())); + offered_qos_profiles.push_back(rosbag2_transport::Rosbag2QoS(info.qos_profile())); } return YAML::Dump(offered_qos_profiles); } diff --git a/rosbag2_transport/test/rosbag2_transport/test_play.cpp b/rosbag2_transport/test/rosbag2_transport/test_play.cpp index e61ca4aa46..7b33abbcdc 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_play.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_play.cpp @@ -471,9 +471,7 @@ class RosBag2PlayQosOverrideTestFixture : public RosBag2PlayTestFixture if (!offered_qos.empty()) { YAML::Node offered_qos_yaml; for (const auto & profile : offered_qos) { - offered_qos_yaml.push_back>( - static_cast>(profile)); + offered_qos_yaml.push_back(profile); } serialized_offered_qos = YAML::Dump(offered_qos_yaml); } diff --git a/rosbag2_transport/test/rosbag2_transport/test_qos.cpp b/rosbag2_transport/test/rosbag2_transport/test_qos.cpp index 6de3b2c462..40b0c3aaf5 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_qos.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_qos.cpp @@ -23,13 +23,14 @@ TEST(TestQoS, serialization) { - rosbag2_transport::Rosbag2QoS_v<> expected_qos; + rosbag2_transport::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 = YAML::decode_for_version>( + loaded_node, 9); ASSERT_EQ(deserialized_profiles.size(), 1u); rosbag2_transport::Rosbag2QoS actual_qos = deserialized_profiles[0]; @@ -58,7 +59,8 @@ 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>>(); + auto deserialized_profiles = YAML::decode_for_version>( + loaded_node, 8); ASSERT_EQ(deserialized_profiles.size(), 1u); auto actual_qos = deserialized_profiles[0].get_rmw_qos_profile(); @@ -134,7 +136,8 @@ TEST(TestQoS, translates_bad_infinity_values) " 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 = YAML::decode_for_version( + loaded_node, 9); 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)); diff --git a/rosbag2_transport/test/rosbag2_transport/test_record_options.cpp b/rosbag2_transport/test/rosbag2_transport/test_record_options.cpp index e305229152..1bdfa21fef 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_record_options.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_record_options.cpp @@ -42,7 +42,8 @@ TEST(record_options, test_yaml_serialization) std::stringstream serializer; serializer << node; auto reconstructed_node = YAML::Load(serializer.str()); - auto reconstructed = reconstructed_node.as(); + auto reconstructed = YAML::decode_for_version( + reconstructed_node, 9); #define CHECK(field) ASSERT_EQ(original.field, reconstructed.field) CHECK(all); From 4208cd34d1b7aad5292da1a06ccbaa0d2aa4201c Mon Sep 17 00:00:00 2001 From: roncapat Date: Sat, 7 Oct 2023 17:36:57 +0200 Subject: [PATCH 07/51] Reduce diff Signed-off-by: Patrick Roncagliolo --- .../src/rosbag2_transport/player.cpp | 4 +- .../src/rosbag2_transport/qos.cpp | 38 +++++++++---------- .../src/rosbag2_transport/recorder.cpp | 2 +- 3 files changed, 20 insertions(+), 24 deletions(-) diff --git a/rosbag2_transport/src/rosbag2_transport/player.cpp b/rosbag2_transport/src/rosbag2_transport/player.cpp index 367c250cf4..f4e77838d3 100644 --- a/rosbag2_transport/src/rosbag2_transport/player.cpp +++ b/rosbag2_transport/src/rosbag2_transport/player.cpp @@ -84,9 +84,7 @@ rclcpp::QoS publisher_qos_for_topic( YAML::decode_for_version>( profiles_yaml, topic.version); - return rosbag2_transport::Rosbag2QoS::adapt_offer_to_recorded_offers( - topic.name, - offered_qos_profiles); + return Rosbag2QoS::adapt_offer_to_recorded_offers(topic.name, offered_qos_profiles); } } // namespace diff --git a/rosbag2_transport/src/rosbag2_transport/qos.cpp b/rosbag2_transport/src/rosbag2_transport/qos.cpp index 0d1bcb0ecf..4204b8c7fe 100644 --- a/rosbag2_transport/src/rosbag2_transport/qos.cpp +++ b/rosbag2_transport/src/rosbag2_transport/qos.cpp @@ -40,7 +40,6 @@ 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 { @@ -132,6 +131,23 @@ bool convert::decode(const Node & node, rmw_time_t & time) return true; } +Node convert::encode( + const rosbag2_transport::Rosbag2QoS & qos) +{ + const auto & p = qos.get_rmw_qos_profile(); + Node node; + node["history"] = convert::encode(p.history); + node["depth"] = p.depth; + node["reliability"] = convert::encode(p.reliability); + node["durability"] = convert::encode(p.durability); + node["deadline"] = p.deadline; + node["lifespan"] = p.lifespan; + node["liveliness"] = convert::encode(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, int version) { @@ -161,23 +177,6 @@ bool convert::decode( .avoid_ros_namespace_conventions(node["avoid_ros_namespace_conventions"].as()); return true; } - -Node convert::encode( - const rosbag2_transport::Rosbag2QoS & qos) -{ - const auto & p = qos.get_rmw_qos_profile(); - Node node; - node["history"] = convert::encode(p.history); - node["depth"] = p.depth; - node["reliability"] = convert::encode(p.reliability); - node["durability"] = convert::encode(p.durability); - node["deadline"] = p.deadline; - node["lifespan"] = p.lifespan; - node["liveliness"] = convert::encode(p.liveliness); - node["liveliness_lease_duration"] = p.liveliness_lease_duration; - node["avoid_ros_namespace_conventions"] = p.avoid_ros_namespace_conventions; - return node; -} } // namespace YAML namespace rosbag2_transport @@ -262,8 +261,7 @@ bool operator==(const rmw_time_t & lhs, const rmw_time_t & rhs) * policies that affect compatibility. * This means it excludes history and lifespan from the equality check. */ -template -bool all_profiles_effectively_same(const std::vector & profiles) +bool all_profiles_effectively_same(const std::vector & profiles) { auto iterator = profiles.begin(); const auto p_ref = iterator->get_rmw_qos_profile(); diff --git a/rosbag2_transport/src/rosbag2_transport/recorder.cpp b/rosbag2_transport/src/rosbag2_transport/recorder.cpp index af3cca8fd0..69c280e136 100644 --- a/rosbag2_transport/src/rosbag2_transport/recorder.cpp +++ b/rosbag2_transport/src/rosbag2_transport/recorder.cpp @@ -508,7 +508,7 @@ std::string RecorderImpl::serialized_offered_qos_profiles_for_topic( { YAML::Node offered_qos_profiles; for (const auto & info : topics_endpoint_info) { - offered_qos_profiles.push_back(rosbag2_transport::Rosbag2QoS(info.qos_profile())); + offered_qos_profiles.push_back(Rosbag2QoS(info.qos_profile())); } return YAML::Dump(offered_qos_profiles); } From 31d98a9528e62aa741eacb0df532c650bab9a834 Mon Sep 17 00:00:00 2001 From: roncapat Date: Sat, 7 Oct 2023 17:48:48 +0200 Subject: [PATCH 08/51] Fix for rosbag2_py Signed-off-by: Patrick Roncagliolo --- rosbag2_py/src/rosbag2_py/_transport.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rosbag2_py/src/rosbag2_py/_transport.cpp b/rosbag2_py/src/rosbag2_py/_transport.cpp index 0237bf5cfa..d637f2dc14 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 = YAML::decode_for_version(bag_node, 9); output_options.push_back(std::make_pair(storage_options, record_options)); } rosbag2_transport::bag_rewrite(input_options, output_options); From a6fb35c6905b3b8e15b2fbb329c86630dd829693 Mon Sep 17 00:00:00 2001 From: roncapat Date: Sat, 7 Oct 2023 17:54:34 +0200 Subject: [PATCH 09/51] Fix missing include (cpplint) Signed-off-by: Patrick Roncagliolo --- rosbag2_transport/include/rosbag2_transport/qos.hpp | 1 + 1 file changed, 1 insertion(+) diff --git a/rosbag2_transport/include/rosbag2_transport/qos.hpp b/rosbag2_transport/include/rosbag2_transport/qos.hpp index dfdd1ad020..a498e87368 100644 --- a/rosbag2_transport/include/rosbag2_transport/qos.hpp +++ b/rosbag2_transport/include/rosbag2_transport/qos.hpp @@ -15,6 +15,7 @@ #ifndef ROSBAG2_TRANSPORT__QOS_HPP_ #define ROSBAG2_TRANSPORT__QOS_HPP_ +#include #include #include From 17281e3f657ecf389faf4ea2a8d8b669cd98bf15 Mon Sep 17 00:00:00 2001 From: roncapat Date: Tue, 24 Oct 2023 21:37:00 +0200 Subject: [PATCH 10/51] WIP: decode everything QoS parsing moved MCAP code already drafted Signed-off-by: Patrick Roncagliolo --- rosbag2_storage/CMakeLists.txt | 3 + .../include/rosbag2_storage}/qos.hpp | 75 ++++++++++++------- .../rosbag2_storage/topic_metadata.hpp | 5 +- .../include/rosbag2_storage/yaml.hpp | 41 ++++------ rosbag2_storage/package.xml | 1 + .../src/rosbag2_storage}/qos.cpp | 24 +++--- .../test_metadata_serialization.cpp | 16 ++-- rosbag2_storage_mcap/src/mcap_storage.cpp | 17 ++++- rosbag2_transport/CMakeLists.txt | 1 - .../src/rosbag2_transport/player.cpp | 6 +- .../src/rosbag2_transport/record_options.cpp | 10 +-- .../test/rosbag2_transport/test_play.cpp | 4 +- .../test/rosbag2_transport/test_qos.cpp | 22 +++--- .../test/rosbag2_transport/test_record.cpp | 2 +- 14 files changed, 127 insertions(+), 100 deletions(-) rename {rosbag2_transport/include/rosbag2_transport => rosbag2_storage/include/rosbag2_storage}/qos.hpp (62%) rename {rosbag2_transport/src/rosbag2_transport => rosbag2_storage/src/rosbag2_storage}/qos.cpp (95%) diff --git a/rosbag2_storage/CMakeLists.txt b/rosbag2_storage/CMakeLists.txt index cd4d7ab601..1dc1b4dadd 100644 --- a/rosbag2_storage/CMakeLists.txt +++ b/rosbag2_storage/CMakeLists.txt @@ -25,11 +25,13 @@ find_package(ament_cmake REQUIRED) find_package(pluginlib REQUIRED) find_package(rcpputils REQUIRED) find_package(rcutils REQUIRED) +find_package(rclcpp 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 +47,7 @@ target_link_libraries(${PROJECT_NAME} pluginlib::pluginlib rcpputils::rcpputils rcutils::rcutils + rclcpp::rclcpp yaml-cpp ) diff --git a/rosbag2_transport/include/rosbag2_transport/qos.hpp b/rosbag2_storage/include/rosbag2_storage/qos.hpp similarity index 62% rename from rosbag2_transport/include/rosbag2_transport/qos.hpp rename to rosbag2_storage/include/rosbag2_storage/qos.hpp index a498e87368..9793e0dd90 100644 --- a/rosbag2_transport/include/rosbag2_transport/qos.hpp +++ b/rosbag2_storage/include/rosbag2_storage/qos.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef ROSBAG2_TRANSPORT__QOS_HPP_ -#define ROSBAG2_TRANSPORT__QOS_HPP_ +#ifndef ROSBAG2_STORAGE__QOS_HPP_ +#define ROSBAG2_STORAGE__QOS_HPP_ #include #include @@ -21,14 +21,15 @@ #include "rclcpp/node_interfaces/node_graph_interface.hpp" #include "rclcpp/qos.hpp" +#include "yaml-cpp/yaml.h" -#include "rosbag2_transport/visibility_control.hpp" -#include "rosbag2_storage/yaml.hpp" -namespace rosbag2_transport +#include "rosbag2_storage/visibility_control.hpp" + +namespace rosbag2_storage { /// Simple wrapper around rclcpp::QoS to provide a default constructor for YAML deserialization. -class ROSBAG2_TRANSPORT_PUBLIC Rosbag2QoS : public rclcpp::QoS +class ROSBAG2_STORAGE_PUBLIC Rosbag2QoS : public rclcpp::QoS { public: Rosbag2QoS() @@ -45,7 +46,7 @@ class ROSBAG2_TRANSPORT_PUBLIC Rosbag2QoS : public rclcpp::QoS // 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. + * - 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 @@ -67,56 +68,80 @@ class ROSBAG2_TRANSPORT_PUBLIC Rosbag2QoS : public rclcpp::QoS const std::string & topic_name, const std::vector & profiles); }; -} // namespace rosbag2_transport +} // 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_TRANSPORT_PUBLIC convert +struct ROSBAG2_STORAGE_PUBLIC convert { static Node encode(const rmw_qos_history_policy_t & policy); static bool decode(const Node & node, rmw_qos_history_policy_t & policy); }; template<> -struct ROSBAG2_TRANSPORT_PUBLIC convert +struct ROSBAG2_STORAGE_PUBLIC convert { static Node encode(const rmw_qos_reliability_policy_t & policy); static bool decode(const Node & node, rmw_qos_reliability_policy_t & policy); }; template<> -struct ROSBAG2_TRANSPORT_PUBLIC convert +struct ROSBAG2_STORAGE_PUBLIC convert { static Node encode(const rmw_qos_durability_policy_t & policy); static bool decode(const Node & node, rmw_qos_durability_policy_t & policy); }; template<> -struct ROSBAG2_TRANSPORT_PUBLIC convert +struct ROSBAG2_STORAGE_PUBLIC convert { static Node encode(const rmw_qos_liveliness_policy_t & policy); static bool decode(const Node & node, rmw_qos_liveliness_policy_t & policy); }; template<> -struct ROSBAG2_TRANSPORT_PUBLIC convert +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_TRANSPORT_PUBLIC convert +struct ROSBAG2_STORAGE_PUBLIC convert { - static Node encode(const rosbag2_transport::Rosbag2QoS & qos); - static bool decode(const Node & node, rosbag2_transport::Rosbag2QoS & qos, int version); + static Node encode(const rosbag2_storage::Rosbag2QoS & qos); + static bool decode(const Node & node, rosbag2_storage::Rosbag2QoS & qos, int version); }; template<> -struct ROSBAG2_TRANSPORT_PUBLIC convert> +struct ROSBAG2_STORAGE_PUBLIC convert> { - static Node encode(const std::vector & rhs) + static Node encode(const std::vector & rhs) { Node node{NodeType::Sequence}; for (const auto & value : rhs) { @@ -126,7 +151,7 @@ struct ROSBAG2_TRANSPORT_PUBLIC convert & rhs, int version) + const Node & node, std::vector & rhs, int version) { if (!node.IsSequence()) { return false; @@ -134,7 +159,7 @@ struct ROSBAG2_TRANSPORT_PUBLIC convert(value, version); + auto temp = decode_for_version(value, version); rhs.push_back(temp); } return true; @@ -142,9 +167,9 @@ struct ROSBAG2_TRANSPORT_PUBLIC convert -struct ROSBAG2_TRANSPORT_PUBLIC convert> +struct ROSBAG2_STORAGE_PUBLIC convert> { - static Node encode(const std::map & rhs) + static Node encode(const std::map & rhs) { Node node{NodeType::Sequence}; for (const auto & [key, value] : rhs) { @@ -154,7 +179,7 @@ struct ROSBAG2_TRANSPORT_PUBLIC convert & rhs, int version) + const Node & node, std::map & rhs, int version) { if (!node.IsMap()) { return false; @@ -162,7 +187,7 @@ struct ROSBAG2_TRANSPORT_PUBLIC convert(element.second, version); + auto temp = decode_for_version(element.second, version); rhs[element.first.as()] = temp; } return true; @@ -170,4 +195,4 @@ struct ROSBAG2_TRANSPORT_PUBLIC convert +#include "rclcpp/qos.hpp" namespace rosbag2_storage { @@ -25,11 +26,9 @@ 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; - int version = 9; bool operator==(const rosbag2_storage::TopicMetadata & rhs) const { diff --git a/rosbag2_storage/include/rosbag2_storage/yaml.hpp b/rosbag2_storage/include/rosbag2_storage/yaml.hpp index 7e6d536a86..5f35dbb7c8 100644 --- a/rosbag2_storage/include/rosbag2_storage/yaml.hpp +++ b/rosbag2_storage/include/rosbag2_storage/yaml.hpp @@ -30,6 +30,7 @@ #endif #include "rosbag2_storage/bag_metadata.hpp" +#include "rosbag2_storage/qos.hpp" namespace YAML { @@ -64,28 +65,6 @@ 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 { @@ -95,21 +74,29 @@ struct convert node["name"] = topic.name; node["type"] = topic.type; node["serialization_format"] = topic.serialization_format; - node["offered_qos_profiles"] = topic.offered_qos_profiles; + std::vector to_encode; + to_encode.reserve(topic.offered_qos_profiles.size()); + std::transform( + topic.offered_qos_profiles.begin(), topic.offered_qos_profiles.end(), to_encode.begin(), + [](auto & qos) {return static_cast(qos);}); + node["offered_qos_profiles"] = convert>::encode( + to_encode); node["type_description_hash"] = topic.type_description_hash; return node; } static bool decode(const Node & node, rosbag2_storage::TopicMetadata & topic, int version) { - topic.version = version; 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 = ""; + auto decoded = + decode_for_version>( + node["offered_qos_profiles"], + version); + topic.offered_qos_profiles.reserve(decoded.size()); + std::copy(decoded.begin(), decoded.end(), topic.offered_qos_profiles.begin()); } if (version >= 7) { topic.type_description_hash = node["type_description_hash"].as(); diff --git a/rosbag2_storage/package.xml b/rosbag2_storage/package.xml index d3086822a8..81242ce1fa 100644 --- a/rosbag2_storage/package.xml +++ b/rosbag2_storage/package.xml @@ -15,6 +15,7 @@ pluginlib rcpputils rcutils + rclcpp yaml_cpp_vendor ament_cmake_gtest diff --git a/rosbag2_transport/src/rosbag2_transport/qos.cpp b/rosbag2_storage/src/rosbag2_storage/qos.cpp similarity index 95% rename from rosbag2_transport/src/rosbag2_transport/qos.cpp rename to rosbag2_storage/src/rosbag2_storage/qos.cpp index 4204b8c7fe..cacd7c28c2 100644 --- a/rosbag2_transport/src/rosbag2_transport/qos.cpp +++ b/rosbag2_storage/src/rosbag2_storage/qos.cpp @@ -16,8 +16,8 @@ #include -#include "rosbag2_transport/qos.hpp" -#include "logging.hpp" +#include "rosbag2_storage/qos.hpp" +#include "rosbag2_storage/logging.hpp" #include "rmw/qos_string_conversions.h" namespace @@ -131,8 +131,8 @@ bool convert::decode(const Node & node, rmw_time_t & time) return true; } -Node convert::encode( - const rosbag2_transport::Rosbag2QoS & qos) +Node convert::encode( + const rosbag2_storage::Rosbag2QoS & qos) { const auto & p = qos.get_rmw_qos_profile(); Node node; @@ -148,8 +148,8 @@ Node convert::encode( return node; } -bool convert::decode( - const Node & node, rosbag2_transport::Rosbag2QoS & qos, int version) +bool convert::decode( + const Node & node, rosbag2_storage::Rosbag2QoS & qos, int version) { if (version <= 8) { auto history = static_cast(node["history"].as()); @@ -179,7 +179,7 @@ bool convert::decode( } } // namespace YAML -namespace rosbag2_transport +namespace rosbag2_storage { Rosbag2QoS Rosbag2QoS::adapt_request_to_offers( const std::string & topic_name, const std::vector & endpoints) @@ -210,7 +210,7 @@ Rosbag2QoS Rosbag2QoS::adapt_request_to_offers( request_qos.reliable(); } else { if (reliability_reliable_endpoints_count > 0) { - ROSBAG2_TRANSPORT_LOG_WARN_STREAM( + 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 " @@ -226,7 +226,7 @@ Rosbag2QoS Rosbag2QoS::adapt_request_to_offers( request_qos.transient_local(); } else { if (durability_transient_local_endpoints_count > 0) { - ROSBAG2_TRANSPORT_LOG_WARN_STREAM( + 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 " @@ -296,10 +296,10 @@ Rosbag2QoS Rosbag2QoS::adapt_offer_to_recorded_offers( return result.default_history(); } - ROSBAG2_TRANSPORT_LOG_WARN_STREAM( + 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_transport default publisher offer."); + "Falling back to the rosbag2_storage default publisher offer."); return Rosbag2QoS{}; } -} // namespace rosbag2_transport +} // 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 d824b35c91..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", "", 8}, 100}); - metadata.topics_with_message_count.push_back({{"topic2", "type2", "rmw2", "qos2", "", 8}, 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,13 +99,14 @@ 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{}; metadata.version = 3; metadata.topics_with_message_count.push_back( - {{"topic", "type", "rmw", offered_qos_profiles, "", 3}, message_count}); + {{"topic", "type", "rmw", offered_qos_profiles, ""}, message_count}); metadata_io_->write_metadata(temporary_dir_path_, metadata); auto read_metadata = metadata_io_->read_metadata(temporary_dir_path_); ASSERT_THAT( @@ -118,13 +119,14 @@ 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{}; metadata.version = 4; metadata.topics_with_message_count.push_back( - {{"topic", "type", "rmw", offered_qos_profiles, "", 4}, message_count}); + {{"topic", "type", "rmw", offered_qos_profiles, ""}, message_count}); metadata_io_->write_metadata(temporary_dir_path_, metadata); auto read_metadata = metadata_io_->read_metadata(temporary_dir_path_); ASSERT_THAT( @@ -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, 7}, 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_storage_mcap/src/mcap_storage.cpp b/rosbag2_storage_mcap/src/mcap_storage.cpp index bcc0269671..d912430779 100644 --- a/rosbag2_storage_mcap/src/mcap_storage.cpp +++ b/rosbag2_storage_mcap/src/mcap_storage.cpp @@ -438,7 +438,10 @@ 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; + auto node = YAML::Load(metadata_it->second); + auto decoded = YAML::decode_for_version>(YAML::Load(metadata_it->second), metadata_.version); + topic_info.topic_metadata.offered_qos_profiles.reserve(decoded.size()); + std::copy(decoded.begin(), decoded.end(), topic_info.topic_metadata.offered_qos_profiles.begin()); } const auto type_hash_it = channel.metadata.find("topic_type_hash"); if (type_hash_it != channel.metadata.end()) { @@ -815,8 +818,16 @@ 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); + + std::vector to_encode; + to_encode.reserve(topic_info.topic_metadata.offered_qos_profiles.size()); + std::transform( + topic_info.topic_metadata.offered_qos_profiles.begin(), + topic_info.topic_metadata.offered_qos_profiles.end(), + to_encode.begin(), + [](auto & qos){return static_cast(qos);}); + auto yaml_node = YAML::convert>::encode(to_encode); + channel.metadata.emplace("offered_qos_profiles", yaml_node.as()); 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_transport/CMakeLists.txt b/rosbag2_transport/CMakeLists.txt index bcd5461478..0dc14ad27d 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 diff --git a/rosbag2_transport/src/rosbag2_transport/player.cpp b/rosbag2_transport/src/rosbag2_transport/player.cpp index f4e77838d3..69745d810d 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( @@ -81,7 +81,7 @@ rclcpp::QoS publisher_qos_for_topic( const auto profiles_yaml = YAML::Load(topic.offered_qos_profiles); const auto offered_qos_profiles = - YAML::decode_for_version>( + YAML::decode_for_version>( profiles_yaml, topic.version); return Rosbag2QoS::adapt_offer_to_recorded_offers(topic.name, offered_qos_profiles); diff --git a/rosbag2_transport/src/rosbag2_transport/record_options.cpp b/rosbag2_transport/src/rosbag2_transport/record_options.cpp index d9ccfeb330..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,11 +54,11 @@ 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"] = convert>::encode(qos_overrides); + rosbag2_storage::Rosbag2QoS>>::encode(qos_overrides); node["include_hidden_topics"] = record_options.include_hidden_topics; node["include_unpublished_topics"] = record_options.include_unpublished_topics; return node; @@ -83,9 +83,9 @@ bool convert::decode( optional_assign(node, "compression_threads", record_options.compression_threads); // yaml-cpp doesn't implement unordered_map - std::map qos_overrides; + std::map qos_overrides; if (node["topic_qos_profile_overrides"]) { - qos_overrides = YAML::decode_for_version>( + 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()); diff --git a/rosbag2_transport/test/rosbag2_transport/test_play.cpp b/rosbag2_transport/test/rosbag2_transport/test_play.cpp index 7b33abbcdc..2db8ae0992 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" @@ -455,7 +455,7 @@ class RosBag2PlayQosOverrideTestFixture : public 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 diff --git a/rosbag2_transport/test/rosbag2_transport/test_qos.cpp b/rosbag2_transport/test/rosbag2_transport/test_qos.cpp index 40b0c3aaf5..7be8204265 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_qos.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_qos.cpp @@ -19,21 +19,21 @@ #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 = YAML::decode_for_version>( + auto deserialized_profiles = YAML::decode_for_version>( loaded_node, 9); 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); } @@ -59,14 +59,14 @@ TEST(TestQoS, supports_version_4) " avoid_ros_namespace_conventions: false\n"; YAML::Node loaded_node = YAML::Load(serialized_profiles); - auto deserialized_profiles = YAML::decode_for_version>( + auto deserialized_profiles = YAML::decode_for_version>( loaded_node, 8); 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() @@ -107,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() @@ -136,7 +136,7 @@ TEST(TestQoS, translates_bad_infinity_values) " nsec: " << infinity.nsec << "\n" "avoid_ros_namespace_conventions: false\n"; const YAML::Node loaded_node = YAML::Load(serialized_profile.str()); - const auto deserialized_profile = YAML::decode_for_version( + const auto deserialized_profile = YAML::decode_for_version( loaded_node, 9); const auto actual_qos = deserialized_profile.get_rmw_qos_profile(); EXPECT_TRUE(rmw_time_equal(actual_qos.lifespan, expected_qos.lifespan)); @@ -147,7 +147,7 @@ TEST(TestQoS, translates_bad_infinity_values) } } -using rosbag2_transport::Rosbag2QoS; // NOLINT +using rosbag2_storage::Rosbag2QoS; // NOLINT class AdaptiveQoSTest : public ::testing::Test { public: @@ -173,8 +173,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 rosbag2_storage::Rosbag2QoS default_offer_{}; + const rosbag2_storage::Rosbag2QoS default_request_{}; }; TEST_F(AdaptiveQoSTest, adapt_request_empty_returns_default) diff --git a/rosbag2_transport/test/rosbag2_transport/test_record.cpp b/rosbag2_transport/test/rosbag2_transport/test_record.cpp index 266bba90a8..5853faf81a 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) From 823dfbade78a343b374373b7525e8af83045da1e Mon Sep 17 00:00:00 2001 From: roncapat Date: Tue, 24 Oct 2023 22:24:38 +0200 Subject: [PATCH 11/51] WIP: decode everything Fix MCAP test compilation Signed-off-by: Patrick Roncagliolo --- .../rosbag2_storage_mcap/test_mcap_storage.cpp | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) 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..1de9308f29 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 @@ -97,9 +97,9 @@ TEST_F(McapStorageTestFixture, can_store_and_read_metadata_correctly) std::vector topics = {"topic1", "topic2"}; rosbag2_storage::TopicMetadata topic_metadata_1 = {topics[0], "std_msgs/msg/String", "cdr", - "qos_profile1", "type_hash1"}; + {rclcpp::QoS(1)}, "type_hash1"}; rosbag2_storage::TopicMetadata topic_metadata_2 = {topics[1], "std_msgs/msg/String", "cdr", - "qos_profile2", "type_hash2"}; + {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(); @@ -234,7 +234,7 @@ TEST_F(TemporaryDirectoryFixture, can_write_and_read_basic_mcap_file) 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"}, + {rclcpp::QoS(1)}, "type_hash1"}, 1u}})); EXPECT_THAT(metadata.message_count, Eq(1u)); From afeee440c7806afc79395ec151b4214618c73c90 Mon Sep 17 00:00:00 2001 From: roncapat Date: Tue, 24 Oct 2023 23:07:41 +0200 Subject: [PATCH 12/51] WIP: decode everything Sqlite3 code Everything builds up to rosbag2_transport (excluded) Signed-off-by: Patrick Roncagliolo --- .../test_sequential_compression_reader.cpp | 2 +- .../test_sequential_compression_writer.cpp | 8 ++-- rosbag2_cpp/src/rosbag2_cpp/reindexer.cpp | 2 +- rosbag2_cpp/test/rosbag2_cpp/fake_data.cpp | 2 +- rosbag2_cpp/test/rosbag2_cpp/test_info.cpp | 12 +++--- .../rosbag2_cpp/test_multifile_reader.cpp | 2 +- .../rosbag2_cpp/test_sequential_writer.cpp | 24 +++++------ .../test_storage_without_metadata_file.cpp | 2 +- .../src/data_generator_executable.cpp | 2 +- .../src/data_generator_node.cpp | 2 +- .../sqlite_storage.cpp | 40 ++++++++++++++----- .../storage_test_fixture.hpp | 2 +- .../test_sqlite_storage.cpp | 28 ++++++------- .../src/rosbag2_transport/bag_rewrite.cpp | 12 ++---- .../src/rosbag2_transport/recorder.cpp | 2 +- 15 files changed, 79 insertions(+), 63 deletions(-) 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..10039d35d8 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); @@ -186,7 +186,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 +195,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 +256,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 +269,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_storage_sqlite3/src/rosbag2_storage_sqlite3/sqlite_storage.cpp b/rosbag2_storage_sqlite3/src/rosbag2_storage_sqlite3/sqlite_storage.cpp index d83e8d5883..aff9365183 100644 --- a/rosbag2_storage_sqlite3/src/rosbag2_storage_sqlite3/sqlite_storage.cpp +++ b/rosbag2_storage_sqlite3/src/rosbag2_storage_sqlite3/sqlite_storage.cpp @@ -460,6 +460,14 @@ void SqliteStorage::create_topic( { std::lock_guard db_lock(database_write_mutex_); if (topics_.find(topic.name) == std::end(topics_)) { + std::vector to_encode; + to_encode.reserve(topic.offered_qos_profiles.size()); + std::transform( + topic.offered_qos_profiles.begin(), + topic.offered_qos_profiles.end(), + to_encode.begin(), + [](auto & qos){return static_cast(qos);}); + auto yaml_node = YAML::convert>::encode(to_encode); auto insert_topic = database_->prepare_statement( "INSERT INTO topics" @@ -469,7 +477,7 @@ void SqliteStorage::create_topic( topic.name, topic.type, topic.serialization_format, - topic.offered_qos_profiles, + yaml_node.as(), topic.type_description_hash); insert_topic->execute_and_reset(); topics_.emplace(topic.name, static_cast(database_->get_last_insert_id())); @@ -591,12 +599,16 @@ void SqliteStorage::fill_topics_and_types() std::string, std::string, std::string, std::string, std::string>(); for (auto result : query_results) { + auto yaml_node = YAML::Load(std::get<3>(result)); + auto decoded = YAML::decode_for_version>(yaml_node, metadata_.version); + std::vector offered_qos_profiles; + std::copy(decoded.begin(), decoded.end(), offered_qos_profiles.begin()); 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 +618,12 @@ void SqliteStorage::fill_topics_and_types() std::string, std::string, std::string, std::string>(); for (auto result : query_results) { + auto yaml_node = YAML::Load(std::get<3>(result)); + auto decoded = YAML::decode_for_version>(yaml_node, metadata_.version); + std::vector offered_qos_profiles; + std::copy(decoded.begin(), decoded.end(), offered_qos_profiles.begin()); 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 +633,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 +697,13 @@ void SqliteStorage::read_metadata() rcutils_time_point_value_t, std::string, std::string>(); for (auto result : query_results) { + auto yaml_node = YAML::Load(std::get<6>(result)); + auto decoded = YAML::decode_for_version>(yaml_node, metadata_.version); + std::vector offered_qos_profiles; + std::copy(decoded.begin(), decoded.end(), offered_qos_profiles.begin()); 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 +724,13 @@ void SqliteStorage::read_metadata() rcutils_time_point_value_t, std::string>(); for (auto result : query_results) { + auto yaml_node = YAML::Load(std::get<6>(result)); + auto decoded = YAML::decode_for_version>(yaml_node, metadata_.version); + std::vector offered_qos_profiles; + std::copy(decoded.begin(), decoded.end(), offered_qos_profiles.begin()); 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 +754,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..c9366e0eb1 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(1)}, "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(1)}, "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(); diff --git a/rosbag2_transport/src/rosbag2_transport/bag_rewrite.cpp b/rosbag2_transport/src/rosbag2_transport/bag_rewrite.cpp index ebf47f1b87..7a924ea3bc 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,7 @@ 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] = topic_metadata.offered_qos_profiles; } // Fill message_definitions_map std::vector msg_definitions; @@ -127,9 +123,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/recorder.cpp b/rosbag2_transport/src/rosbag2_transport/recorder.cpp index 69c280e136..b95b3c46de 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" From 0b7fc66cbd4c8a297f53a4c5e50d5626cea55971 Mon Sep 17 00:00:00 2001 From: roncapat Date: Tue, 24 Oct 2023 23:45:39 +0200 Subject: [PATCH 13/51] WIP: decode everything rosbag2_transport partial rework Signed-off-by: Patrick Roncagliolo --- .../test_sqlite_storage.cpp | 2 +- .../src/rosbag2_transport/player.cpp | 16 +++--- .../src/rosbag2_transport/recorder.cpp | 20 ++++---- .../rosbag2_play_duration_until_fixture.hpp | 4 +- .../test/rosbag2_transport/test_burst.cpp | 16 +++--- .../test_keyboard_controls.cpp | 4 +- .../test/rosbag2_transport/test_play.cpp | 51 +++++++++---------- .../rosbag2_transport/test_play_callbacks.cpp | 2 +- .../rosbag2_transport/test_play_duration.cpp | 6 +-- .../test/rosbag2_transport/test_play_loop.cpp | 4 +- .../test/rosbag2_transport/test_play_next.cpp | 14 ++--- .../test_play_publish_clock.cpp | 2 +- .../test/rosbag2_transport/test_play_seek.cpp | 2 +- .../rosbag2_transport/test_play_services.cpp | 2 +- .../rosbag2_transport/test_play_timing.cpp | 2 +- .../test_play_topic_remap.cpp | 2 +- .../rosbag2_transport/test_play_until.cpp | 8 +-- .../rosbag2_transport/test_player_stop.cpp | 2 +- .../test/rosbag2_transport/test_qos.cpp | 36 ++++++------- 19 files changed, 95 insertions(+), 100 deletions(-) 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 c9366e0eb1..4b418992f0 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 @@ -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/src/rosbag2_transport/player.cpp b/rosbag2_transport/src/rosbag2_transport/player.cpp index 69745d810d..0294c3d025 100644 --- a/rosbag2_transport/src/rosbag2_transport/player.cpp +++ b/rosbag2_transport/src/rosbag2_transport/player.cpp @@ -74,17 +74,17 @@ rclcpp::QoS publisher_qos_for_topic( RCLCPP_INFO_STREAM( logger, "Overriding QoS profile for topic " << topic.name); - return Rosbag2QoS{qos_it->second}; + return rosbag2_storage::Rosbag2QoS{qos_it->second}; } else if (topic.offered_qos_profiles.empty()) { - return Rosbag2QoS{}; + return rosbag2_storage::Rosbag2QoS{}; } - const auto profiles_yaml = YAML::Load(topic.offered_qos_profiles); - const auto offered_qos_profiles = - YAML::decode_for_version>( - profiles_yaml, - topic.version); - return Rosbag2QoS::adapt_offer_to_recorded_offers(topic.name, offered_qos_profiles); + std::vector casted; + casted.reserve(topic.offered_qos_profiles.size()); + std::transform( + topic.offered_qos_profiles.begin(), topic.offered_qos_profiles.end(), casted.begin(), + [](auto & qos) {return static_cast(qos);}); + return rosbag2_storage::Rosbag2QoS::adapt_offer_to_recorded_offers(topic.name, casted); } } // namespace diff --git a/rosbag2_transport/src/rosbag2_transport/recorder.cpp b/rosbag2_transport/src/rosbag2_transport/recorder.cpp index b95b3c46de..f47e1c64ce 100644 --- a/rosbag2_transport/src/rosbag2_transport/recorder.cpp +++ b/rosbag2_transport/src/rosbag2_transport/recorder.cpp @@ -103,15 +103,15 @@ class RecorderImpl * Find the QoS profile that should be used for subscribing. * * Uses the override from record_options, if it is specified for this topic. - * Otherwise, falls back to Rosbag2QoS::adapt_request_to_offers + * Otherwise, falls back to rosbag2_storage::Rosbag2QoS::adapt_request_to_offers * * \param topic_name The full name of the topic, with namespace (ex. /arm/joint_status). * \return The QoS profile to be used for subscribing. */ 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 2db8ae0992..1e5c77c1f4 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_play.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_play.cpp @@ -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 = @@ -467,16 +467,11 @@ 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, ""}); + std::vector casted; + casted.reserve(offered_qos.size()); + std::copy(offered_qos.begin(), offered_qos.end(), casted.begin()); + + topic_types_.push_back({topic_name_, msg_type_, "", casted, ""}); } template @@ -567,7 +562,7 @@ TEST_F(RosBag2PlayQosOverrideTestFixture, playback_uses_recorded_transient_local // In this test, we subscribe requesting DURABILITY_TRANSIENT_LOCAL. // The bag metadata has this recorded for the original Publisher, // so playback's offer should be compatible (whereas the default offer would not be) - const auto transient_local_profile = Rosbag2QoS{Rosbag2QoS{}.transient_local()}; + const auto transient_local_profile = rosbag2_storage::Rosbag2QoS{rosbag2_storage::Rosbag2QoS{}.transient_local()}; // This should normally take less than 1s - just making it shorter than 60s default const auto timeout = 5s; @@ -590,8 +585,8 @@ TEST_F(RosBag2PlayQosOverrideTestFixture, playback_uses_recorded_deadline) // requirement, so they are compatible. const rclcpp::Duration request_deadline{1s}; const rclcpp::Duration offer_deadline{500ms}; - const auto request_profile = Rosbag2QoS{}.deadline(request_deadline); - const auto offer_profile = Rosbag2QoS{Rosbag2QoS{}.deadline(offer_deadline)}; + const auto request_profile = rosbag2_storage::Rosbag2QoS{}.deadline(request_deadline); + const auto offer_profile = rosbag2_storage::Rosbag2QoS{rosbag2_storage::Rosbag2QoS{}.deadline(offer_deadline)}; const auto timeout = 5s; initialize({offer_profile}); @@ -612,10 +607,10 @@ TEST_F(RosBag2PlayQosOverrideTestFixture, override_has_precedence_over_recorded) const rclcpp::Duration override_liveliness_offer{250ms}; ASSERT_LT(liveliness_request, recorded_liveliness_offer); ASSERT_LT(override_liveliness_offer, liveliness_request); - const auto request_profile = Rosbag2QoS{}.liveliness_lease_duration(liveliness_request); - const auto recorded_offer_profile = Rosbag2QoS{Rosbag2QoS{}.liveliness_lease_duration( + const auto request_profile = rosbag2_storage::Rosbag2QoS{}.liveliness_lease_duration(liveliness_request); + const auto recorded_offer_profile = rosbag2_storage::Rosbag2QoS{rosbag2_storage::Rosbag2QoS{}.liveliness_lease_duration( recorded_liveliness_offer)}; - const auto override_offer_profile = Rosbag2QoS{Rosbag2QoS{}.liveliness_lease_duration( + const auto override_offer_profile = rosbag2_storage::Rosbag2QoS{rosbag2_storage::Rosbag2QoS{}.liveliness_lease_duration( override_liveliness_offer)}; const auto topic_qos_profile_overrides = std::unordered_map{ std::pair{topic_name_, override_offer_profile}, @@ -636,7 +631,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_qos.cpp b/rosbag2_transport/test/rosbag2_transport/test_qos.cpp index 7be8204265..7645ad00d2 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_qos.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_qos.cpp @@ -179,7 +179,7 @@ class AdaptiveQoSTest : public ::testing::Test TEST_F(AdaptiveQoSTest, adapt_request_empty_returns_default) { - auto adapted_request = Rosbag2QoS::adapt_request_to_offers(topic_name_, endpoints_); + auto adapted_request = rosbag2_storage::Rosbag2QoS::adapt_request_to_offers(topic_name_, endpoints_); EXPECT_EQ(default_request_, adapted_request); } @@ -187,10 +187,10 @@ TEST_F(AdaptiveQoSTest, adapt_request_single_offer_returns_same_values) { // Set up this offer to use nondefault reliability and durability, // expect to see those values in the output - auto nondefault_offer = Rosbag2QoS{}.best_effort().transient_local(); + auto nondefault_offer = rosbag2_storage::Rosbag2QoS{}.best_effort().transient_local(); add_endpoint(nondefault_offer); - auto adapted_request = Rosbag2QoS::adapt_request_to_offers(topic_name_, endpoints_); + auto adapted_request = rosbag2_storage::Rosbag2QoS::adapt_request_to_offers(topic_name_, endpoints_); auto expected = nondefault_offer.get_rmw_qos_profile(); auto actual = adapted_request.get_rmw_qos_profile(); @@ -200,13 +200,13 @@ TEST_F(AdaptiveQoSTest, adapt_request_single_offer_returns_same_values) TEST_F(AdaptiveQoSTest, adapt_request_multiple_similar_offers_returns_same_values) { - auto nondefault_offer = Rosbag2QoS{}.best_effort().transient_local(); + auto nondefault_offer = rosbag2_storage::Rosbag2QoS{}.best_effort().transient_local(); const size_t num_endpoints{3}; for (size_t i = 0; i < num_endpoints; i++) { add_endpoint(nondefault_offer); } - auto adapted_request = Rosbag2QoS::adapt_request_to_offers(topic_name_, endpoints_); + auto adapted_request = rosbag2_storage::Rosbag2QoS::adapt_request_to_offers(topic_name_, endpoints_); auto expected = nondefault_offer.get_rmw_qos_profile(); auto actual = adapted_request.get_rmw_qos_profile(); @@ -218,7 +218,7 @@ TEST_F(AdaptiveQoSTest, adapt_request_mixed_reliability_offers_return_best_effor { add_endpoint(Rosbag2QoS{}.best_effort()); add_endpoint(Rosbag2QoS{}.reliable()); - auto adapted_request = Rosbag2QoS::adapt_request_to_offers(topic_name_, endpoints_); + auto adapted_request = rosbag2_storage::Rosbag2QoS::adapt_request_to_offers(topic_name_, endpoints_); EXPECT_EQ( adapted_request.get_rmw_qos_profile().reliability, RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT); } @@ -227,13 +227,13 @@ TEST_F(AdaptiveQoSTest, adapt_request_mixed_durability_offers_return_volatile) { add_endpoint(Rosbag2QoS{}.transient_local()); add_endpoint(Rosbag2QoS{}.durability_volatile()); - auto adapted_request = Rosbag2QoS::adapt_request_to_offers(topic_name_, endpoints_); + auto adapted_request = rosbag2_storage::Rosbag2QoS::adapt_request_to_offers(topic_name_, endpoints_); EXPECT_EQ(adapted_request.get_rmw_qos_profile().durability, RMW_QOS_POLICY_DURABILITY_VOLATILE); } TEST_F(AdaptiveQoSTest, adapt_offer_empty_returns_default) { - auto adapted_offer = Rosbag2QoS::adapt_offer_to_recorded_offers(topic_name_, {}); + auto adapted_offer = rosbag2_storage::Rosbag2QoS::adapt_offer_to_recorded_offers(topic_name_, {}); EXPECT_EQ(adapted_offer, default_offer_); } @@ -241,17 +241,17 @@ TEST_F(AdaptiveQoSTest, adapt_offer_single_offer_returns_same_values) { // Set up this offer to use nondefault reliability and durability, // expect to see those values in the output - auto nondefault_offer = Rosbag2QoS{Rosbag2QoS{}.best_effort().transient_local()}; + auto nondefault_offer = rosbag2_storage::Rosbag2QoS{rosbag2_storage::Rosbag2QoS{}.best_effort().transient_local()}; std::vector offers = {nondefault_offer}; - auto adapted_offer = Rosbag2QoS::adapt_offer_to_recorded_offers(topic_name_, offers); + auto adapted_offer = rosbag2_storage::Rosbag2QoS::adapt_offer_to_recorded_offers(topic_name_, offers); EXPECT_EQ(nondefault_offer, adapted_offer); } TEST_F(AdaptiveQoSTest, adapt_offer_multiple_offers_with_same_settings_return_identical) { - auto nondefault_offer = Rosbag2QoS{Rosbag2QoS{}.best_effort().transient_local()}; - auto adapted_offer = Rosbag2QoS::adapt_offer_to_recorded_offers( + auto nondefault_offer = rosbag2_storage::Rosbag2QoS{rosbag2_storage::Rosbag2QoS{}.best_effort().transient_local()}; + auto adapted_offer = rosbag2_storage::Rosbag2QoS::adapt_offer_to_recorded_offers( topic_name_, {nondefault_offer, nondefault_offer, nondefault_offer}); EXPECT_EQ(nondefault_offer, adapted_offer); } @@ -261,10 +261,10 @@ TEST_F(AdaptiveQoSTest, adapt_offer_mixed_compatibility_returns_default) // When the offers have mixed values for policies that affect compatibility, // it should fall back to the default. std::vector offers = { - Rosbag2QoS{Rosbag2QoS{}.best_effort()}, - Rosbag2QoS{Rosbag2QoS{}.reliable()}, + rosbag2_storage::Rosbag2QoS{rosbag2_storage::Rosbag2QoS{}.best_effort()}, + rosbag2_storage::Rosbag2QoS{rosbag2_storage::Rosbag2QoS{}.reliable()}, }; - auto adapted_offer = Rosbag2QoS::adapt_offer_to_recorded_offers(topic_name_, offers); + auto adapted_offer = rosbag2_storage::Rosbag2QoS::adapt_offer_to_recorded_offers(topic_name_, offers); EXPECT_EQ(adapted_offer, default_offer_); } @@ -275,9 +275,9 @@ TEST_F(AdaptiveQoSTest, adapt_offer_mixed_non_compatibility_returns_first) rclcpp::Duration nonstandard_duration(12, 34); size_t nonstandard_history{20}; std::vector offers = { - Rosbag2QoS{Rosbag2QoS{}.lifespan(nonstandard_duration)}, - Rosbag2QoS{Rosbag2QoS{}.keep_last(nonstandard_history)}, + rosbag2_storage::Rosbag2QoS{rosbag2_storage::Rosbag2QoS{}.lifespan(nonstandard_duration)}, + rosbag2_storage::Rosbag2QoS{rosbag2_storage::Rosbag2QoS{}.keep_last(nonstandard_history)}, }; - auto adapted_offer = Rosbag2QoS::adapt_offer_to_recorded_offers(topic_name_, offers); + auto adapted_offer = rosbag2_storage::Rosbag2QoS::adapt_offer_to_recorded_offers(topic_name_, offers); EXPECT_EQ(adapted_offer, offers[0]); } From e4164d64329986da90290e97d45debe4a5523d20 Mon Sep 17 00:00:00 2001 From: roncapat Date: Wed, 25 Oct 2023 21:11:39 +0200 Subject: [PATCH 14/51] WIP: decode everything rosbag2_transport compiles Signed-off-by: Patrick Roncagliolo --- .../test/rosbag2_transport/test_record.cpp | 10 +++++++++- .../test/rosbag2_transport/test_rewrite.cpp | 4 +--- 2 files changed, 10 insertions(+), 4 deletions(-) diff --git a/rosbag2_transport/test/rosbag2_transport/test_record.cpp b/rosbag2_transport/test/rosbag2_transport/test_record.cpp index 5853faf81a..0f7f9cc0e3 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_record.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_record.cpp @@ -175,7 +175,15 @@ 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::vector to_encode; + to_encode.reserve(offered_qos_profiles.size()); + std::transform( + offered_qos_profiles.begin(), offered_qos_profiles.end(), to_encode.begin(), + [](auto & qos) {return static_cast(qos);}); + std::string serialized_profiles = YAML::convert>::encode(to_encode).as(); // Basic smoke test that the profile was serialized into the metadata as a string. EXPECT_THAT(serialized_profiles, ContainsRegex("reliability: reliable\n")); EXPECT_THAT(serialized_profiles, ContainsRegex("durability: volatile\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); } } } From b3c1040b6f4c82d19b3060acd5c4b179ba1f8e1d Mon Sep 17 00:00:00 2001 From: roncapat Date: Wed, 25 Oct 2023 21:58:32 +0200 Subject: [PATCH 15/51] WIP: decode everything everything compiles Signed-off-by: roncapat --- rosbag2_py/src/rosbag2_py/_storage.cpp | 55 +++++++++++++++++++++++++- 1 file changed, 53 insertions(+), 2 deletions(-) diff --git a/rosbag2_py/src/rosbag2_py/_storage.cpp b/rosbag2_py/src/rosbag2_py/_storage.cpp index 56a719bb38..120300f112 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,63 @@ 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") = {}, pybind11::arg("type_description_hash") = "") .def_readwrite("name", &rosbag2_storage::TopicMetadata::name) .def_readwrite("type", &rosbag2_storage::TopicMetadata::type) From 9a9f9a27d0f598114acee99e69fce250d2fadfea Mon Sep 17 00:00:00 2001 From: roncapat Date: Wed, 25 Oct 2023 21:59:50 +0200 Subject: [PATCH 16/51] Uncrustify (warning: still draft code) Signed-off-by: roncapat --- rosbag2_py/src/rosbag2_py/_storage.cpp | 36 ++++++++++-------- .../test/rosbag2_transport/test_play.cpp | 17 ++++++--- .../test/rosbag2_transport/test_qos.cpp | 38 ++++++++++++++----- .../test/rosbag2_transport/test_record.cpp | 3 +- 4 files changed, 62 insertions(+), 32 deletions(-) diff --git a/rosbag2_py/src/rosbag2_py/_storage.cpp b/rosbag2_py/src/rosbag2_py/_storage.cpp index 120300f112..90f4b21ca6 100644 --- a/rosbag2_py/src/rosbag2_py/_storage.cpp +++ b/rosbag2_py/src/rosbag2_py/_storage.cpp @@ -169,14 +169,14 @@ PYBIND11_MODULE(_storage, m) { .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); + .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); + .value("RMW_QOS_POLICY_LIVELINESS_UNKNOWN", RMW_QOS_POLICY_LIVELINESS_UNKNOWN); pybind11::class_(m, "Duration") .def( @@ -188,20 +188,26 @@ PYBIND11_MODULE(_storage, m) { .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("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)); + .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( diff --git a/rosbag2_transport/test/rosbag2_transport/test_play.cpp b/rosbag2_transport/test/rosbag2_transport/test_play.cpp index 1e5c77c1f4..d00f1480af 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_play.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_play.cpp @@ -470,7 +470,7 @@ class RosBag2PlayQosOverrideTestFixture : public RosBag2PlayTestFixture std::vector casted; casted.reserve(offered_qos.size()); std::copy(offered_qos.begin(), offered_qos.end(), casted.begin()); - + topic_types_.push_back({topic_name_, msg_type_, "", casted, ""}); } @@ -562,7 +562,8 @@ TEST_F(RosBag2PlayQosOverrideTestFixture, playback_uses_recorded_transient_local // In this test, we subscribe requesting DURABILITY_TRANSIENT_LOCAL. // The bag metadata has this recorded for the original Publisher, // so playback's offer should be compatible (whereas the default offer would not be) - const auto transient_local_profile = rosbag2_storage::Rosbag2QoS{rosbag2_storage::Rosbag2QoS{}.transient_local()}; + const auto transient_local_profile = + rosbag2_storage::Rosbag2QoS{rosbag2_storage::Rosbag2QoS{}.transient_local()}; // This should normally take less than 1s - just making it shorter than 60s default const auto timeout = 5s; @@ -586,7 +587,8 @@ TEST_F(RosBag2PlayQosOverrideTestFixture, playback_uses_recorded_deadline) const rclcpp::Duration request_deadline{1s}; const rclcpp::Duration offer_deadline{500ms}; const auto request_profile = rosbag2_storage::Rosbag2QoS{}.deadline(request_deadline); - const auto offer_profile = rosbag2_storage::Rosbag2QoS{rosbag2_storage::Rosbag2QoS{}.deadline(offer_deadline)}; + const auto offer_profile = rosbag2_storage::Rosbag2QoS{rosbag2_storage::Rosbag2QoS{}.deadline( + offer_deadline)}; const auto timeout = 5s; initialize({offer_profile}); @@ -607,10 +609,13 @@ TEST_F(RosBag2PlayQosOverrideTestFixture, override_has_precedence_over_recorded) const rclcpp::Duration override_liveliness_offer{250ms}; ASSERT_LT(liveliness_request, recorded_liveliness_offer); ASSERT_LT(override_liveliness_offer, liveliness_request); - const auto request_profile = rosbag2_storage::Rosbag2QoS{}.liveliness_lease_duration(liveliness_request); - const auto recorded_offer_profile = rosbag2_storage::Rosbag2QoS{rosbag2_storage::Rosbag2QoS{}.liveliness_lease_duration( + const auto request_profile = rosbag2_storage::Rosbag2QoS{}.liveliness_lease_duration( + liveliness_request); + const auto recorded_offer_profile = + rosbag2_storage::Rosbag2QoS{rosbag2_storage::Rosbag2QoS{}.liveliness_lease_duration( recorded_liveliness_offer)}; - const auto override_offer_profile = rosbag2_storage::Rosbag2QoS{rosbag2_storage::Rosbag2QoS{}.liveliness_lease_duration( + const auto override_offer_profile = + rosbag2_storage::Rosbag2QoS{rosbag2_storage::Rosbag2QoS{}.liveliness_lease_duration( override_liveliness_offer)}; const auto topic_qos_profile_overrides = std::unordered_map{ std::pair{topic_name_, override_offer_profile}, diff --git a/rosbag2_transport/test/rosbag2_transport/test_qos.cpp b/rosbag2_transport/test/rosbag2_transport/test_qos.cpp index 7645ad00d2..fdc5e8a809 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_qos.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_qos.cpp @@ -179,7 +179,9 @@ class AdaptiveQoSTest : public ::testing::Test TEST_F(AdaptiveQoSTest, adapt_request_empty_returns_default) { - auto adapted_request = rosbag2_storage::Rosbag2QoS::adapt_request_to_offers(topic_name_, endpoints_); + auto adapted_request = rosbag2_storage::Rosbag2QoS::adapt_request_to_offers( + topic_name_, + endpoints_); EXPECT_EQ(default_request_, adapted_request); } @@ -190,7 +192,9 @@ TEST_F(AdaptiveQoSTest, adapt_request_single_offer_returns_same_values) auto nondefault_offer = rosbag2_storage::Rosbag2QoS{}.best_effort().transient_local(); add_endpoint(nondefault_offer); - auto adapted_request = rosbag2_storage::Rosbag2QoS::adapt_request_to_offers(topic_name_, endpoints_); + auto adapted_request = rosbag2_storage::Rosbag2QoS::adapt_request_to_offers( + topic_name_, + endpoints_); auto expected = nondefault_offer.get_rmw_qos_profile(); auto actual = adapted_request.get_rmw_qos_profile(); @@ -206,7 +210,9 @@ TEST_F(AdaptiveQoSTest, adapt_request_multiple_similar_offers_returns_same_value add_endpoint(nondefault_offer); } - auto adapted_request = rosbag2_storage::Rosbag2QoS::adapt_request_to_offers(topic_name_, endpoints_); + auto adapted_request = rosbag2_storage::Rosbag2QoS::adapt_request_to_offers( + topic_name_, + endpoints_); auto expected = nondefault_offer.get_rmw_qos_profile(); auto actual = adapted_request.get_rmw_qos_profile(); @@ -218,7 +224,9 @@ TEST_F(AdaptiveQoSTest, adapt_request_mixed_reliability_offers_return_best_effor { add_endpoint(Rosbag2QoS{}.best_effort()); add_endpoint(Rosbag2QoS{}.reliable()); - auto adapted_request = rosbag2_storage::Rosbag2QoS::adapt_request_to_offers(topic_name_, endpoints_); + auto adapted_request = rosbag2_storage::Rosbag2QoS::adapt_request_to_offers( + topic_name_, + endpoints_); EXPECT_EQ( adapted_request.get_rmw_qos_profile().reliability, RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT); } @@ -227,7 +235,9 @@ TEST_F(AdaptiveQoSTest, adapt_request_mixed_durability_offers_return_volatile) { add_endpoint(Rosbag2QoS{}.transient_local()); add_endpoint(Rosbag2QoS{}.durability_volatile()); - auto adapted_request = rosbag2_storage::Rosbag2QoS::adapt_request_to_offers(topic_name_, endpoints_); + auto adapted_request = rosbag2_storage::Rosbag2QoS::adapt_request_to_offers( + topic_name_, + endpoints_); EXPECT_EQ(adapted_request.get_rmw_qos_profile().durability, RMW_QOS_POLICY_DURABILITY_VOLATILE); } @@ -241,16 +251,20 @@ TEST_F(AdaptiveQoSTest, adapt_offer_single_offer_returns_same_values) { // Set up this offer to use nondefault reliability and durability, // expect to see those values in the output - auto nondefault_offer = rosbag2_storage::Rosbag2QoS{rosbag2_storage::Rosbag2QoS{}.best_effort().transient_local()}; + auto nondefault_offer = + rosbag2_storage::Rosbag2QoS{rosbag2_storage::Rosbag2QoS{}.best_effort().transient_local()}; std::vector offers = {nondefault_offer}; - auto adapted_offer = rosbag2_storage::Rosbag2QoS::adapt_offer_to_recorded_offers(topic_name_, offers); + auto adapted_offer = rosbag2_storage::Rosbag2QoS::adapt_offer_to_recorded_offers( + topic_name_, + offers); EXPECT_EQ(nondefault_offer, adapted_offer); } TEST_F(AdaptiveQoSTest, adapt_offer_multiple_offers_with_same_settings_return_identical) { - auto nondefault_offer = rosbag2_storage::Rosbag2QoS{rosbag2_storage::Rosbag2QoS{}.best_effort().transient_local()}; + auto nondefault_offer = + rosbag2_storage::Rosbag2QoS{rosbag2_storage::Rosbag2QoS{}.best_effort().transient_local()}; auto adapted_offer = rosbag2_storage::Rosbag2QoS::adapt_offer_to_recorded_offers( topic_name_, {nondefault_offer, nondefault_offer, nondefault_offer}); EXPECT_EQ(nondefault_offer, adapted_offer); @@ -264,7 +278,9 @@ TEST_F(AdaptiveQoSTest, adapt_offer_mixed_compatibility_returns_default) rosbag2_storage::Rosbag2QoS{rosbag2_storage::Rosbag2QoS{}.best_effort()}, rosbag2_storage::Rosbag2QoS{rosbag2_storage::Rosbag2QoS{}.reliable()}, }; - auto adapted_offer = rosbag2_storage::Rosbag2QoS::adapt_offer_to_recorded_offers(topic_name_, offers); + auto adapted_offer = rosbag2_storage::Rosbag2QoS::adapt_offer_to_recorded_offers( + topic_name_, + offers); EXPECT_EQ(adapted_offer, default_offer_); } @@ -278,6 +294,8 @@ TEST_F(AdaptiveQoSTest, adapt_offer_mixed_non_compatibility_returns_first) rosbag2_storage::Rosbag2QoS{rosbag2_storage::Rosbag2QoS{}.lifespan(nonstandard_duration)}, rosbag2_storage::Rosbag2QoS{rosbag2_storage::Rosbag2QoS{}.keep_last(nonstandard_history)}, }; - auto adapted_offer = rosbag2_storage::Rosbag2QoS::adapt_offer_to_recorded_offers(topic_name_, offers); + auto adapted_offer = rosbag2_storage::Rosbag2QoS::adapt_offer_to_recorded_offers( + topic_name_, + offers); EXPECT_EQ(adapted_offer, offers[0]); } diff --git a/rosbag2_transport/test/rosbag2_transport/test_record.cpp b/rosbag2_transport/test/rosbag2_transport/test_record.cpp index 0f7f9cc0e3..391030ff64 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_record.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_record.cpp @@ -183,7 +183,8 @@ TEST_F(RecordIntegrationTestFixture, qos_is_stored_in_metadata) std::transform( offered_qos_profiles.begin(), offered_qos_profiles.end(), to_encode.begin(), [](auto & qos) {return static_cast(qos);}); - std::string serialized_profiles = YAML::convert>::encode(to_encode).as(); + std::string serialized_profiles = YAML::convert>::encode( + to_encode).as(); // Basic smoke test that the profile was serialized into the metadata as a string. EXPECT_THAT(serialized_profiles, ContainsRegex("reliability: reliable\n")); EXPECT_THAT(serialized_profiles, ContainsRegex("durability: volatile\n")); From 57cc668004f26e605285ece45c58a4500ffa84fb Mon Sep 17 00:00:00 2001 From: roncapat Date: Wed, 25 Oct 2023 22:04:59 +0200 Subject: [PATCH 17/51] Uncrustify Signed-off-by: roncapat --- .../sqlite_storage.cpp | 27 +++++++++++++------ 1 file changed, 19 insertions(+), 8 deletions(-) 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 aff9365183..6244ebe48b 100644 --- a/rosbag2_storage_sqlite3/src/rosbag2_storage_sqlite3/sqlite_storage.cpp +++ b/rosbag2_storage_sqlite3/src/rosbag2_storage_sqlite3/sqlite_storage.cpp @@ -466,7 +466,7 @@ void SqliteStorage::create_topic( topic.offered_qos_profiles.begin(), topic.offered_qos_profiles.end(), to_encode.begin(), - [](auto & qos){return static_cast(qos);}); + [](auto & qos) {return static_cast(qos);}); auto yaml_node = YAML::convert>::encode(to_encode); auto insert_topic = database_->prepare_statement( @@ -600,7 +600,9 @@ void SqliteStorage::fill_topics_and_types() for (auto result : query_results) { auto yaml_node = YAML::Load(std::get<3>(result)); - auto decoded = YAML::decode_for_version>(yaml_node, metadata_.version); + auto decoded = YAML::decode_for_version>( + yaml_node, + metadata_.version); std::vector offered_qos_profiles; std::copy(decoded.begin(), decoded.end(), offered_qos_profiles.begin()); all_topics_and_types_.push_back( @@ -619,11 +621,14 @@ void SqliteStorage::fill_topics_and_types() for (auto result : query_results) { auto yaml_node = YAML::Load(std::get<3>(result)); - auto decoded = YAML::decode_for_version>(yaml_node, metadata_.version); + auto decoded = YAML::decode_for_version>( + yaml_node, + metadata_.version); std::vector offered_qos_profiles; std::copy(decoded.begin(), decoded.end(), offered_qos_profiles.begin()); all_topics_and_types_.push_back( - {std::get<0>(result), std::get<1>(result), std::get<2>(result), offered_qos_profiles, ""}); + {std::get<0>(result), std::get<1>(result), std::get<2>(result), offered_qos_profiles, + ""}); } } } else { @@ -698,12 +703,15 @@ void SqliteStorage::read_metadata() for (auto result : query_results) { auto yaml_node = YAML::Load(std::get<6>(result)); - auto decoded = YAML::decode_for_version>(yaml_node, metadata_.version); + auto decoded = YAML::decode_for_version>( + yaml_node, + metadata_.version); std::vector offered_qos_profiles; std::copy(decoded.begin(), decoded.end(), offered_qos_profiles.begin()); metadata_.topics_with_message_count.push_back( { - {std::get<0>(result), std::get<1>(result), std::get<2>(result), offered_qos_profiles, 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)) }); @@ -725,12 +733,15 @@ void SqliteStorage::read_metadata() for (auto result : query_results) { auto yaml_node = YAML::Load(std::get<6>(result)); - auto decoded = YAML::decode_for_version>(yaml_node, metadata_.version); + auto decoded = YAML::decode_for_version>( + yaml_node, + metadata_.version); std::vector offered_qos_profiles; std::copy(decoded.begin(), decoded.end(), offered_qos_profiles.begin()); metadata_.topics_with_message_count.push_back( { - {std::get<0>(result), std::get<1>(result), std::get<2>(result), offered_qos_profiles, ""}, + {std::get<0>(result), std::get<1>(result), std::get<2>( + result), offered_qos_profiles, ""}, static_cast(std::get<3>(result)) }); From 827df9d88d1343692bed9d885e69b2ab0a4eb7e5 Mon Sep 17 00:00:00 2001 From: roncapat Date: Wed, 25 Oct 2023 22:07:14 +0200 Subject: [PATCH 18/51] Try to export dependency Signed-off-by: roncapat --- rosbag2_storage/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rosbag2_storage/CMakeLists.txt b/rosbag2_storage/CMakeLists.txt index 1dc1b4dadd..66f7d0a65d 100644 --- a/rosbag2_storage/CMakeLists.txt +++ b/rosbag2_storage/CMakeLists.txt @@ -73,7 +73,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) if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) From e0483dc03b8b2596454bba477be759305f13a700 Mon Sep 17 00:00:00 2001 From: roncapat Date: Wed, 25 Oct 2023 22:18:28 +0200 Subject: [PATCH 19/51] Cpplint Signed-off-by: roncapat --- .../rosbag2_storage/topic_metadata.hpp | 1 + .../include/rosbag2_storage/yaml.hpp | 1 + rosbag2_storage_mcap/src/mcap_storage.cpp | 19 +++++++++++-------- 3 files changed, 13 insertions(+), 8 deletions(-) diff --git a/rosbag2_storage/include/rosbag2_storage/topic_metadata.hpp b/rosbag2_storage/include/rosbag2_storage/topic_metadata.hpp index 32b9b8afdd..fec92b6127 100644 --- a/rosbag2_storage/include/rosbag2_storage/topic_metadata.hpp +++ b/rosbag2_storage/include/rosbag2_storage/topic_metadata.hpp @@ -16,6 +16,7 @@ #define ROSBAG2_STORAGE__TOPIC_METADATA_HPP_ #include +#include #include "rclcpp/qos.hpp" namespace rosbag2_storage diff --git a/rosbag2_storage/include/rosbag2_storage/yaml.hpp b/rosbag2_storage/include/rosbag2_storage/yaml.hpp index 5f35dbb7c8..abf8ed2f11 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 diff --git a/rosbag2_storage_mcap/src/mcap_storage.cpp b/rosbag2_storage_mcap/src/mcap_storage.cpp index d912430779..3b38e8f7fd 100644 --- a/rosbag2_storage_mcap/src/mcap_storage.cpp +++ b/rosbag2_storage_mcap/src/mcap_storage.cpp @@ -439,9 +439,11 @@ void MCAPStorage::read_metadata() const auto metadata_it = channel.metadata.find("offered_qos_profiles"); if (metadata_it != channel.metadata.end()) { auto node = YAML::Load(metadata_it->second); - auto decoded = YAML::decode_for_version>(YAML::Load(metadata_it->second), metadata_.version); + auto decoded = YAML::decode_for_version>( + YAML::Load(metadata_it->second), metadata_.version); topic_info.topic_metadata.offered_qos_profiles.reserve(decoded.size()); - std::copy(decoded.begin(), decoded.end(), topic_info.topic_metadata.offered_qos_profiles.begin()); + std::copy(decoded.begin(), decoded.end(), + topic_info.topic_metadata.offered_qos_profiles.begin()); } const auto type_hash_it = channel.metadata.find("topic_type_hash"); if (type_hash_it != channel.metadata.end()) { @@ -818,14 +820,15 @@ 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; - + std::vector to_encode; to_encode.reserve(topic_info.topic_metadata.offered_qos_profiles.size()); - std::transform( - topic_info.topic_metadata.offered_qos_profiles.begin(), - topic_info.topic_metadata.offered_qos_profiles.end(), - to_encode.begin(), - [](auto & qos){return static_cast(qos);}); + std::transform(topic_info.topic_metadata.offered_qos_profiles.begin(), + topic_info.topic_metadata.offered_qos_profiles.end(), + to_encode.begin(), + [](auto & qos) { + return static_cast(qos); + }); auto yaml_node = YAML::convert>::encode(to_encode); channel.metadata.emplace("offered_qos_profiles", yaml_node.as()); channel.metadata.emplace("topic_type_hash", topic_info.topic_metadata.type_description_hash); From 1359a4740de76ca0b0e092deb3ea61be1d1c100d Mon Sep 17 00:00:00 2001 From: roncapat Date: Wed, 25 Oct 2023 23:12:22 +0200 Subject: [PATCH 20/51] Fix encode bug Signed-off-by: roncapat --- rosbag2_storage/include/rosbag2_storage/yaml.hpp | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/rosbag2_storage/include/rosbag2_storage/yaml.hpp b/rosbag2_storage/include/rosbag2_storage/yaml.hpp index abf8ed2f11..b55639ff32 100644 --- a/rosbag2_storage/include/rosbag2_storage/yaml.hpp +++ b/rosbag2_storage/include/rosbag2_storage/yaml.hpp @@ -78,10 +78,11 @@ struct convert std::vector to_encode; to_encode.reserve(topic.offered_qos_profiles.size()); std::transform( - topic.offered_qos_profiles.begin(), topic.offered_qos_profiles.end(), to_encode.begin(), + topic.offered_qos_profiles.begin(), topic.offered_qos_profiles.end(), std::back_inserter(to_encode), [](auto & qos) {return static_cast(qos);}); - node["offered_qos_profiles"] = convert>::encode( + auto temp = convert>::encode( to_encode); + node["offered_qos_profiles"] = YAML::Dump(temp); node["type_description_hash"] = topic.type_description_hash; return node; } @@ -92,10 +93,10 @@ struct convert topic.type = node["type"].as(); topic.serialization_format = node["serialization_format"].as(); if (version >= 4) { + std::string qos_str = node["offered_qos_profiles"].as(); auto decoded = decode_for_version>( - node["offered_qos_profiles"], - version); + YAML::Load(qos_str), version); topic.offered_qos_profiles.reserve(decoded.size()); std::copy(decoded.begin(), decoded.end(), topic.offered_qos_profiles.begin()); } From acf1fc23c65570e760e85f45f9a5af08d0d2fb44 Mon Sep 17 00:00:00 2001 From: roncapat Date: Wed, 25 Oct 2023 23:22:42 +0200 Subject: [PATCH 21/51] Fix transform bug Signed-off-by: roncapat --- rosbag2_storage_mcap/src/mcap_storage.cpp | 2 +- .../src/rosbag2_storage_sqlite3/sqlite_storage.cpp | 2 +- rosbag2_transport/src/rosbag2_transport/player.cpp | 2 +- rosbag2_transport/test/rosbag2_transport/test_record.cpp | 2 +- 4 files changed, 4 insertions(+), 4 deletions(-) diff --git a/rosbag2_storage_mcap/src/mcap_storage.cpp b/rosbag2_storage_mcap/src/mcap_storage.cpp index 3b38e8f7fd..7641df0fba 100644 --- a/rosbag2_storage_mcap/src/mcap_storage.cpp +++ b/rosbag2_storage_mcap/src/mcap_storage.cpp @@ -825,7 +825,7 @@ void MCAPStorage::create_topic(const rosbag2_storage::TopicMetadata & topic, to_encode.reserve(topic_info.topic_metadata.offered_qos_profiles.size()); std::transform(topic_info.topic_metadata.offered_qos_profiles.begin(), topic_info.topic_metadata.offered_qos_profiles.end(), - to_encode.begin(), + std::back_inserter(to_encode), [](auto & qos) { return static_cast(qos); }); 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 6244ebe48b..219e024d14 100644 --- a/rosbag2_storage_sqlite3/src/rosbag2_storage_sqlite3/sqlite_storage.cpp +++ b/rosbag2_storage_sqlite3/src/rosbag2_storage_sqlite3/sqlite_storage.cpp @@ -465,7 +465,7 @@ void SqliteStorage::create_topic( std::transform( topic.offered_qos_profiles.begin(), topic.offered_qos_profiles.end(), - to_encode.begin(), + std::back_inserter(to_encode), [](auto & qos) {return static_cast(qos);}); auto yaml_node = YAML::convert>::encode(to_encode); auto insert_topic = diff --git a/rosbag2_transport/src/rosbag2_transport/player.cpp b/rosbag2_transport/src/rosbag2_transport/player.cpp index 0294c3d025..52d6657a1c 100644 --- a/rosbag2_transport/src/rosbag2_transport/player.cpp +++ b/rosbag2_transport/src/rosbag2_transport/player.cpp @@ -82,7 +82,7 @@ rclcpp::QoS publisher_qos_for_topic( std::vector casted; casted.reserve(topic.offered_qos_profiles.size()); std::transform( - topic.offered_qos_profiles.begin(), topic.offered_qos_profiles.end(), casted.begin(), + topic.offered_qos_profiles.begin(), topic.offered_qos_profiles.end(), std::back_inserter(casted), [](auto & qos) {return static_cast(qos);}); return rosbag2_storage::Rosbag2QoS::adapt_offer_to_recorded_offers(topic.name, casted); } diff --git a/rosbag2_transport/test/rosbag2_transport/test_record.cpp b/rosbag2_transport/test/rosbag2_transport/test_record.cpp index 391030ff64..f100aaacd8 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_record.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_record.cpp @@ -181,7 +181,7 @@ TEST_F(RecordIntegrationTestFixture, qos_is_stored_in_metadata) std::vector to_encode; to_encode.reserve(offered_qos_profiles.size()); std::transform( - offered_qos_profiles.begin(), offered_qos_profiles.end(), to_encode.begin(), + offered_qos_profiles.begin(), offered_qos_profiles.end(), std::back_inserter(to_encode), [](auto & qos) {return static_cast(qos);}); std::string serialized_profiles = YAML::convert>::encode( to_encode).as(); From 95521bb899bea27f31d795b73101ba4d59a92963 Mon Sep 17 00:00:00 2001 From: roncapat Date: Wed, 25 Oct 2023 23:54:58 +0200 Subject: [PATCH 22/51] Try to fix runtime error with py constructor Signed-off-by: roncapat --- rosbag2_py/src/rosbag2_py/_storage.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rosbag2_py/src/rosbag2_py/_storage.cpp b/rosbag2_py/src/rosbag2_py/_storage.cpp index 90f4b21ca6..8d9c7285ec 100644 --- a/rosbag2_py/src/rosbag2_py/_storage.cpp +++ b/rosbag2_py/src/rosbag2_py/_storage.cpp @@ -215,7 +215,7 @@ PYBIND11_MODULE(_storage, m) { 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) From 5083ebbac8913ad16a7fde31d3dd6c106b3027b3 Mon Sep 17 00:00:00 2001 From: Patrick Roncagliolo Date: Thu, 26 Oct 2023 18:27:24 +0200 Subject: [PATCH 23/51] Fix "test_record" Signed-off-by: Patrick Roncagliolo --- rosbag2_transport/test/rosbag2_transport/test_record.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/rosbag2_transport/test/rosbag2_transport/test_record.cpp b/rosbag2_transport/test/rosbag2_transport/test_record.cpp index f100aaacd8..4bfcd9a405 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_record.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_record.cpp @@ -183,8 +183,8 @@ TEST_F(RecordIntegrationTestFixture, qos_is_stored_in_metadata) std::transform( offered_qos_profiles.begin(), offered_qos_profiles.end(), std::back_inserter(to_encode), [](auto & qos) {return static_cast(qos);}); - std::string serialized_profiles = YAML::convert>::encode( - to_encode).as(); + auto temp = YAML::convert>::encode(to_encode); + std::string serialized_profiles = YAML::Dump(temp); // Basic smoke test that the profile was serialized into the metadata as a string. EXPECT_THAT(serialized_profiles, ContainsRegex("reliability: reliable\n")); EXPECT_THAT(serialized_profiles, ContainsRegex("durability: volatile\n")); From f20226b47652445e5b210e86c38b1b3f44bedcc3 Mon Sep 17 00:00:00 2001 From: Patrick Roncagliolo Date: Thu, 26 Oct 2023 18:38:41 +0200 Subject: [PATCH 24/51] Various fixes Signed-off-by: Patrick Roncagliolo --- rosbag2_storage/include/rosbag2_storage/yaml.hpp | 3 ++- rosbag2_storage_mcap/src/mcap_storage.cpp | 4 ++-- .../src/rosbag2_storage_sqlite3/sqlite_storage.cpp | 2 +- rosbag2_transport/src/rosbag2_transport/player.cpp | 3 ++- 4 files changed, 7 insertions(+), 5 deletions(-) diff --git a/rosbag2_storage/include/rosbag2_storage/yaml.hpp b/rosbag2_storage/include/rosbag2_storage/yaml.hpp index b55639ff32..f5e1b9f17f 100644 --- a/rosbag2_storage/include/rosbag2_storage/yaml.hpp +++ b/rosbag2_storage/include/rosbag2_storage/yaml.hpp @@ -78,7 +78,8 @@ struct convert std::vector to_encode; to_encode.reserve(topic.offered_qos_profiles.size()); std::transform( - topic.offered_qos_profiles.begin(), topic.offered_qos_profiles.end(), std::back_inserter(to_encode), + topic.offered_qos_profiles.begin(), topic.offered_qos_profiles.end(), + std::back_inserter(to_encode), [](auto & qos) {return static_cast(qos);}); auto temp = convert>::encode( to_encode); diff --git a/rosbag2_storage_mcap/src/mcap_storage.cpp b/rosbag2_storage_mcap/src/mcap_storage.cpp index 7641df0fba..08afd7f215 100644 --- a/rosbag2_storage_mcap/src/mcap_storage.cpp +++ b/rosbag2_storage_mcap/src/mcap_storage.cpp @@ -440,7 +440,7 @@ void MCAPStorage::read_metadata() if (metadata_it != channel.metadata.end()) { auto node = YAML::Load(metadata_it->second); auto decoded = YAML::decode_for_version>( - YAML::Load(metadata_it->second), metadata_.version); + YAML::Load(metadata_it->second), metadata_.version); topic_info.topic_metadata.offered_qos_profiles.reserve(decoded.size()); std::copy(decoded.begin(), decoded.end(), topic_info.topic_metadata.offered_qos_profiles.begin()); @@ -830,7 +830,7 @@ void MCAPStorage::create_topic(const rosbag2_storage::TopicMetadata & topic, return static_cast(qos); }); auto yaml_node = YAML::convert>::encode(to_encode); - channel.metadata.emplace("offered_qos_profiles", yaml_node.as()); + channel.metadata.emplace("offered_qos_profiles", YAML::Dump(yaml_node)); 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_sqlite3/src/rosbag2_storage_sqlite3/sqlite_storage.cpp b/rosbag2_storage_sqlite3/src/rosbag2_storage_sqlite3/sqlite_storage.cpp index 219e024d14..5b810c8bd4 100644 --- a/rosbag2_storage_sqlite3/src/rosbag2_storage_sqlite3/sqlite_storage.cpp +++ b/rosbag2_storage_sqlite3/src/rosbag2_storage_sqlite3/sqlite_storage.cpp @@ -477,7 +477,7 @@ void SqliteStorage::create_topic( topic.name, topic.type, topic.serialization_format, - yaml_node.as(), + YAML::Dump(yaml_node), topic.type_description_hash); insert_topic->execute_and_reset(); topics_.emplace(topic.name, static_cast(database_->get_last_insert_id())); diff --git a/rosbag2_transport/src/rosbag2_transport/player.cpp b/rosbag2_transport/src/rosbag2_transport/player.cpp index 52d6657a1c..b0cb494817 100644 --- a/rosbag2_transport/src/rosbag2_transport/player.cpp +++ b/rosbag2_transport/src/rosbag2_transport/player.cpp @@ -82,7 +82,8 @@ rclcpp::QoS publisher_qos_for_topic( std::vector casted; casted.reserve(topic.offered_qos_profiles.size()); std::transform( - topic.offered_qos_profiles.begin(), topic.offered_qos_profiles.end(), std::back_inserter(casted), + topic.offered_qos_profiles.begin(), topic.offered_qos_profiles.end(), + std::back_inserter(casted), [](auto & qos) {return static_cast(qos);}); return rosbag2_storage::Rosbag2QoS::adapt_offer_to_recorded_offers(topic.name, casted); } From b080d4f7d5b0bafba4f18389f50f4f4151bf219e Mon Sep 17 00:00:00 2001 From: Patrick Roncagliolo Date: Thu, 26 Oct 2023 18:54:40 +0200 Subject: [PATCH 25/51] Various fixes Signed-off-by: Patrick Roncagliolo --- .../src/rosbag2_storage_sqlite3/sqlite_storage.cpp | 9 +++++---- .../test/rosbag2_storage_sqlite3/test_sqlite_storage.cpp | 4 ++-- 2 files changed, 7 insertions(+), 6 deletions(-) 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 5b810c8bd4..d8c48d3c95 100644 --- a/rosbag2_storage_sqlite3/src/rosbag2_storage_sqlite3/sqlite_storage.cpp +++ b/rosbag2_storage_sqlite3/src/rosbag2_storage_sqlite3/sqlite_storage.cpp @@ -604,7 +604,8 @@ void SqliteStorage::fill_topics_and_types() yaml_node, metadata_.version); std::vector offered_qos_profiles; - std::copy(decoded.begin(), decoded.end(), offered_qos_profiles.begin()); + offered_qos_profiles.reserve(decoded.size()); + std::copy(decoded.begin(), decoded.end(), std::back_inserter(offered_qos_profiles)); all_topics_and_types_.push_back( { std::get<0>(result), @@ -625,7 +626,7 @@ void SqliteStorage::fill_topics_and_types() yaml_node, metadata_.version); std::vector offered_qos_profiles; - std::copy(decoded.begin(), decoded.end(), offered_qos_profiles.begin()); + std::copy(decoded.begin(), decoded.end(), std::back_inserter(offered_qos_profiles)); all_topics_and_types_.push_back( {std::get<0>(result), std::get<1>(result), std::get<2>(result), offered_qos_profiles, ""}); @@ -707,7 +708,7 @@ void SqliteStorage::read_metadata() yaml_node, metadata_.version); std::vector offered_qos_profiles; - std::copy(decoded.begin(), decoded.end(), offered_qos_profiles.begin()); + std::copy(decoded.begin(), decoded.end(), std::back_inserter(offered_qos_profiles)); metadata_.topics_with_message_count.push_back( { {std::get<0>(result), std::get<1>(result), std::get<2>( @@ -737,7 +738,7 @@ void SqliteStorage::read_metadata() yaml_node, metadata_.version); std::vector offered_qos_profiles; - std::copy(decoded.begin(), decoded.end(), offered_qos_profiles.begin()); + std::copy(decoded.begin(), decoded.end(), std::back_inserter(offered_qos_profiles)); metadata_.topics_with_message_count.push_back( { {std::get<0>(result), std::get<1>(result), std::get<2>( 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 4b418992f0..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 @@ -191,7 +191,7 @@ TEST_F(StorageTestFixture, get_all_topics_and_types_returns_the_correct_vector) topics_and_types, ElementsAreArray( { rosbag2_storage::TopicMetadata{"topic1", "type1", "rmw1", {rclcpp::QoS(1)}, "type_hash1"}, - rosbag2_storage::TopicMetadata{"topic2", "type2", "rmw2", {rclcpp::QoS(1)}, "type_hash2"} + rosbag2_storage::TopicMetadata{"topic2", "type2", "rmw2", {rclcpp::QoS(2)}, "type_hash2"} })); } @@ -269,7 +269,7 @@ TEST_F(StorageTestFixture, get_metadata_returns_correct_struct) { rosbag2_storage::TopicInformation{rosbag2_storage::TopicMetadata{ "topic1", "type1", "rmw1", {rclcpp::QoS(1)}, "type_hash1"}, 2u}, rosbag2_storage::TopicInformation{rosbag2_storage::TopicMetadata{ - "topic2", "type2", "rmw2", {rclcpp::QoS(1)}, "type_hash2"}, 1u} + "topic2", "type2", "rmw2", {rclcpp::QoS(2)}, "type_hash2"}, 1u} })); EXPECT_THAT(metadata.message_count, Eq(3u)); EXPECT_THAT( From 00520e8ccfd82b17b989446f3b9f5bd1273c5d60 Mon Sep 17 00:00:00 2001 From: Patrick Roncagliolo Date: Thu, 26 Oct 2023 19:12:42 +0200 Subject: [PATCH 26/51] Various fixes Signed-off-by: Patrick Roncagliolo --- rosbag2_transport/test/rosbag2_transport/test_play.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rosbag2_transport/test/rosbag2_transport/test_play.cpp b/rosbag2_transport/test/rosbag2_transport/test_play.cpp index d00f1480af..cf9b73c6ee 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_play.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_play.cpp @@ -469,7 +469,7 @@ class RosBag2PlayQosOverrideTestFixture : public RosBag2PlayTestFixture std::vector casted; casted.reserve(offered_qos.size()); - std::copy(offered_qos.begin(), offered_qos.end(), casted.begin()); + std::copy(offered_qos.begin(), offered_qos.end(), std::back_inserter(casted)); topic_types_.push_back({topic_name_, msg_type_, "", casted, ""}); } From e22afad26da8b09e05bb413359cbdb469d7a6b0c Mon Sep 17 00:00:00 2001 From: Patrick Roncagliolo Date: Thu, 26 Oct 2023 19:16:02 +0200 Subject: [PATCH 27/51] Various fixes Signed-off-by: Patrick Roncagliolo --- rosbag2_storage/include/rosbag2_storage/yaml.hpp | 2 +- rosbag2_storage_mcap/src/mcap_storage.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/rosbag2_storage/include/rosbag2_storage/yaml.hpp b/rosbag2_storage/include/rosbag2_storage/yaml.hpp index f5e1b9f17f..7c4b763fba 100644 --- a/rosbag2_storage/include/rosbag2_storage/yaml.hpp +++ b/rosbag2_storage/include/rosbag2_storage/yaml.hpp @@ -99,7 +99,7 @@ struct convert decode_for_version>( YAML::Load(qos_str), version); topic.offered_qos_profiles.reserve(decoded.size()); - std::copy(decoded.begin(), decoded.end(), topic.offered_qos_profiles.begin()); + std::copy(decoded.begin(), decoded.end(), std::back_inserter(topic.offered_qos_profiles)); } if (version >= 7) { topic.type_description_hash = node["type_description_hash"].as(); diff --git a/rosbag2_storage_mcap/src/mcap_storage.cpp b/rosbag2_storage_mcap/src/mcap_storage.cpp index 08afd7f215..f17c82a3c1 100644 --- a/rosbag2_storage_mcap/src/mcap_storage.cpp +++ b/rosbag2_storage_mcap/src/mcap_storage.cpp @@ -443,7 +443,7 @@ void MCAPStorage::read_metadata() YAML::Load(metadata_it->second), metadata_.version); topic_info.topic_metadata.offered_qos_profiles.reserve(decoded.size()); std::copy(decoded.begin(), decoded.end(), - topic_info.topic_metadata.offered_qos_profiles.begin()); + std::back_inserter(topic_info.topic_metadata.offered_qos_profiles)); } const auto type_hash_it = channel.metadata.find("topic_type_hash"); if (type_hash_it != channel.metadata.end()) { From fbd5846a5053682d9750e7b6372b06f263cbb060 Mon Sep 17 00:00:00 2001 From: Patrick Roncagliolo Date: Thu, 26 Oct 2023 19:24:18 +0200 Subject: [PATCH 28/51] Format Signed-off-by: Patrick Roncagliolo --- rosbag2_storage_mcap/src/mcap_storage.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/rosbag2_storage_mcap/src/mcap_storage.cpp b/rosbag2_storage_mcap/src/mcap_storage.cpp index f17c82a3c1..eda656595f 100644 --- a/rosbag2_storage_mcap/src/mcap_storage.cpp +++ b/rosbag2_storage_mcap/src/mcap_storage.cpp @@ -443,7 +443,7 @@ void MCAPStorage::read_metadata() YAML::Load(metadata_it->second), metadata_.version); topic_info.topic_metadata.offered_qos_profiles.reserve(decoded.size()); std::copy(decoded.begin(), decoded.end(), - std::back_inserter(topic_info.topic_metadata.offered_qos_profiles)); + std::back_inserter(topic_info.topic_metadata.offered_qos_profiles)); } const auto type_hash_it = channel.metadata.find("topic_type_hash"); if (type_hash_it != channel.metadata.end()) { @@ -824,11 +824,11 @@ void MCAPStorage::create_topic(const rosbag2_storage::TopicMetadata & topic, std::vector to_encode; to_encode.reserve(topic_info.topic_metadata.offered_qos_profiles.size()); std::transform(topic_info.topic_metadata.offered_qos_profiles.begin(), - topic_info.topic_metadata.offered_qos_profiles.end(), - std::back_inserter(to_encode), - [](auto & qos) { - return static_cast(qos); - }); + topic_info.topic_metadata.offered_qos_profiles.end(), + std::back_inserter(to_encode), + [](auto & qos) { + return static_cast(qos); + }); auto yaml_node = YAML::convert>::encode(to_encode); channel.metadata.emplace("offered_qos_profiles", YAML::Dump(yaml_node)); channel.metadata.emplace("topic_type_hash", topic_info.topic_metadata.type_description_hash); From a7a7356b1561bbab31ffe028a6a607618f849d40 Mon Sep 17 00:00:00 2001 From: Patrick Roncagliolo Date: Thu, 26 Oct 2023 19:28:41 +0200 Subject: [PATCH 29/51] Format Signed-off-by: Patrick Roncagliolo --- rosbag2_storage_mcap/src/mcap_storage.cpp | 5 ++--- .../test/rosbag2_storage_mcap/test_mcap_storage.cpp | 12 ++++++------ 2 files changed, 8 insertions(+), 9 deletions(-) diff --git a/rosbag2_storage_mcap/src/mcap_storage.cpp b/rosbag2_storage_mcap/src/mcap_storage.cpp index eda656595f..02422d455c 100644 --- a/rosbag2_storage_mcap/src/mcap_storage.cpp +++ b/rosbag2_storage_mcap/src/mcap_storage.cpp @@ -825,9 +825,8 @@ void MCAPStorage::create_topic(const rosbag2_storage::TopicMetadata & topic, to_encode.reserve(topic_info.topic_metadata.offered_qos_profiles.size()); std::transform(topic_info.topic_metadata.offered_qos_profiles.begin(), topic_info.topic_metadata.offered_qos_profiles.end(), - std::back_inserter(to_encode), - [](auto & qos) { - return static_cast(qos); + std::back_inserter(to_encode), [](auto & qos) { + return static_cast(qos); }); auto yaml_node = YAML::convert>::encode(to_encode); channel.metadata.emplace("offered_qos_profiles", YAML::Dump(yaml_node)); 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 1de9308f29..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", - {rclcpp::QoS(1)}, "type_hash1"}; - rosbag2_storage::TopicMetadata topic_metadata_2 = {topics[1], "std_msgs/msg/String", "cdr", - {rclcpp::QoS(2)}, "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> @@ -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", - {rclcpp::QoS(1)}, "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)); From 258fb5613f76587f3a864fdc79a0f0927050d0f1 Mon Sep 17 00:00:00 2001 From: Patrick Roncagliolo Date: Fri, 27 Oct 2023 10:22:31 +0200 Subject: [PATCH 30/51] Tentative fixes Signed-off-by: Patrick Roncagliolo --- rosbag2_storage/include/rosbag2_storage/yaml.hpp | 12 +++++++----- .../rosbag2_storage_sqlite3/sqlite_storage.cpp | 16 ++++++++++------ 2 files changed, 17 insertions(+), 11 deletions(-) diff --git a/rosbag2_storage/include/rosbag2_storage/yaml.hpp b/rosbag2_storage/include/rosbag2_storage/yaml.hpp index 7c4b763fba..ebd3cd7547 100644 --- a/rosbag2_storage/include/rosbag2_storage/yaml.hpp +++ b/rosbag2_storage/include/rosbag2_storage/yaml.hpp @@ -95,11 +95,13 @@ struct convert topic.serialization_format = node["serialization_format"].as(); if (version >= 4) { std::string qos_str = node["offered_qos_profiles"].as(); - auto decoded = - decode_for_version>( - YAML::Load(qos_str), version); - topic.offered_qos_profiles.reserve(decoded.size()); - std::copy(decoded.begin(), decoded.end(), std::back_inserter(topic.offered_qos_profiles)); + if (qos_str != ""){ + auto decoded = + decode_for_version>( + YAML::Load(qos_str), version); + topic.offered_qos_profiles.reserve(decoded.size()); + std::copy(decoded.begin(), decoded.end(), std::back_inserter(topic.offered_qos_profiles)); + } } if (version >= 7) { topic.type_description_hash = node["type_description_hash"].as(); 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 d8c48d3c95..c174a3aa9a 100644 --- a/rosbag2_storage_sqlite3/src/rosbag2_storage_sqlite3/sqlite_storage.cpp +++ b/rosbag2_storage_sqlite3/src/rosbag2_storage_sqlite3/sqlite_storage.cpp @@ -733,12 +733,16 @@ void SqliteStorage::read_metadata() rcutils_time_point_value_t, std::string>(); for (auto result : query_results) { - auto yaml_node = YAML::Load(std::get<6>(result)); - auto decoded = YAML::decode_for_version>( - yaml_node, - metadata_.version); - std::vector offered_qos_profiles; - std::copy(decoded.begin(), decoded.end(), std::back_inserter(offered_qos_profiles)); + std::vector offered_qos_profiles {}; + std::string yaml_str = std::get<6>(result); + if (yaml_str != ""){ + auto yaml_node = YAML::Load(yaml_str); + auto decoded = YAML::decode_for_version>( + yaml_node, + metadata_.version); + offered_qos_profiles.reserve(decoded.size()); + std::copy(decoded.begin(), decoded.end(), std::back_inserter(offered_qos_profiles)); + } metadata_.topics_with_message_count.push_back( { {std::get<0>(result), std::get<1>(result), std::get<2>( From 3c24d68d66069c4a75e5cb6404532eaed9b2bf44 Mon Sep 17 00:00:00 2001 From: Patrick Roncagliolo Date: Fri, 27 Oct 2023 10:40:40 +0200 Subject: [PATCH 31/51] Fixes Signed-off-by: Patrick Roncagliolo --- rosbag2_storage/include/rosbag2_storage/yaml.hpp | 2 +- .../src/rosbag2_storage_sqlite3/sqlite_storage.cpp | 2 +- rosbag2_transport/src/rosbag2_transport/bag_rewrite.cpp | 6 +++++- 3 files changed, 7 insertions(+), 3 deletions(-) diff --git a/rosbag2_storage/include/rosbag2_storage/yaml.hpp b/rosbag2_storage/include/rosbag2_storage/yaml.hpp index ebd3cd7547..636e7d0315 100644 --- a/rosbag2_storage/include/rosbag2_storage/yaml.hpp +++ b/rosbag2_storage/include/rosbag2_storage/yaml.hpp @@ -95,7 +95,7 @@ struct convert topic.serialization_format = node["serialization_format"].as(); if (version >= 4) { std::string qos_str = node["offered_qos_profiles"].as(); - if (qos_str != ""){ + if (qos_str != "") { auto decoded = decode_for_version>( YAML::Load(qos_str), version); 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 c174a3aa9a..d6e46e684f 100644 --- a/rosbag2_storage_sqlite3/src/rosbag2_storage_sqlite3/sqlite_storage.cpp +++ b/rosbag2_storage_sqlite3/src/rosbag2_storage_sqlite3/sqlite_storage.cpp @@ -735,7 +735,7 @@ void SqliteStorage::read_metadata() for (auto result : query_results) { std::vector offered_qos_profiles {}; std::string yaml_str = std::get<6>(result); - if (yaml_str != ""){ + if (yaml_str != "") { auto yaml_node = YAML::Load(yaml_str); auto decoded = YAML::decode_for_version>( yaml_node, diff --git a/rosbag2_transport/src/rosbag2_transport/bag_rewrite.cpp b/rosbag2_transport/src/rosbag2_transport/bag_rewrite.cpp index 7a924ea3bc..0ea9771b10 100644 --- a/rosbag2_transport/src/rosbag2_transport/bag_rewrite.cpp +++ b/rosbag2_transport/src/rosbag2_transport/bag_rewrite.cpp @@ -96,7 +96,11 @@ setup_topic_filtering( // Gather all offered qos profiles from all inputs input_topics_qos_profiles.try_emplace(topic_name); - input_topics_qos_profiles[topic_name] = topic_metadata.offered_qos_profiles; + 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; From e1dda74f94d3b5830267c7dfbf77cbf72502fc7d Mon Sep 17 00:00:00 2001 From: Patrick Roncagliolo Date: Fri, 27 Oct 2023 11:10:04 +0200 Subject: [PATCH 32/51] MCAP Fixes Signed-off-by: Patrick Roncagliolo --- rosbag2_storage_mcap/src/mcap_storage.cpp | 14 ++++++++------ 1 file changed, 8 insertions(+), 6 deletions(-) diff --git a/rosbag2_storage_mcap/src/mcap_storage.cpp b/rosbag2_storage_mcap/src/mcap_storage.cpp index 02422d455c..fb436d55f2 100644 --- a/rosbag2_storage_mcap/src/mcap_storage.cpp +++ b/rosbag2_storage_mcap/src/mcap_storage.cpp @@ -438,12 +438,14 @@ 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()) { - auto node = YAML::Load(metadata_it->second); - auto decoded = YAML::decode_for_version>( - YAML::Load(metadata_it->second), metadata_.version); - topic_info.topic_metadata.offered_qos_profiles.reserve(decoded.size()); - std::copy(decoded.begin(), decoded.end(), - std::back_inserter(topic_info.topic_metadata.offered_qos_profiles)); + if (metadata_it->second != "") { + auto node = YAML::Load(metadata_it->second); + auto decoded = YAML::decode_for_version>( + node, metadata_.version); + topic_info.topic_metadata.offered_qos_profiles.reserve(decoded.size()); + std::copy(decoded.begin(), decoded.end(), + std::back_inserter(topic_info.topic_metadata.offered_qos_profiles)); + } } const auto type_hash_it = channel.metadata.find("topic_type_hash"); if (type_hash_it != channel.metadata.end()) { From 189e25bf3bdf310d78222d06e4b821a35fe69664 Mon Sep 17 00:00:00 2001 From: Patrick Roncagliolo Date: Fri, 27 Oct 2023 12:46:20 +0200 Subject: [PATCH 33/51] History QOS Fixes Signed-off-by: Patrick Roncagliolo --- rosbag2_storage/src/rosbag2_storage/qos.cpp | 35 ++++++++++++++++----- 1 file changed, 27 insertions(+), 8 deletions(-) diff --git a/rosbag2_storage/src/rosbag2_storage/qos.cpp b/rosbag2_storage/src/rosbag2_storage/qos.cpp index cacd7c28c2..5504205785 100644 --- a/rosbag2_storage/src/rosbag2_storage/qos.cpp +++ b/rosbag2_storage/src/rosbag2_storage/qos.cpp @@ -151,30 +151,49 @@ Node convert::encode( bool convert::decode( const Node & node, rosbag2_storage::Rosbag2QoS & qos, int version) { + qos + .deadline(node["deadline"].as()) + .lifespan(node["lifespan"].as()) + .liveliness_lease_duration(node["liveliness_lease_duration"].as()) + .avoid_ros_namespace_conventions(node["avoid_ros_namespace_conventions"].as()); + if (version <= 8) { 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()); + switch(history) { + case RMW_QOS_POLICY_HISTORY_KEEP_LAST: + qos.keep_last(node["depth"].as()); + break; + case RMW_QOS_POLICY_HISTORY_KEEP_ALL: + qos.keep_all(); + break; + default: + qos.history(history); + } qos - .history(history) .reliability(reliability) .durability(durability) .liveliness(liveliness); } else { + auto history = node["history"].as(); + switch(history) { + case RMW_QOS_POLICY_HISTORY_KEEP_LAST: + qos.keep_last(node["depth"].as()); + break; + case RMW_QOS_POLICY_HISTORY_KEEP_ALL: + qos.keep_all(); + break; + default: + qos.history(history); + } qos - .history(node["history"].as()) .reliability(node["reliability"].as()) .durability(node["durability"].as()) .liveliness(node["liveliness"].as()); } - qos - .keep_last(node["depth"].as()) - .deadline(node["deadline"].as()) - .lifespan(node["lifespan"].as()) - .liveliness_lease_duration(node["liveliness_lease_duration"].as()) - .avoid_ros_namespace_conventions(node["avoid_ros_namespace_conventions"].as()); return true; } } // namespace YAML From 526f28e5c01392e2ef913f64941ebad69550c10f Mon Sep 17 00:00:00 2001 From: Patrick Roncagliolo Date: Fri, 27 Oct 2023 12:51:44 +0200 Subject: [PATCH 34/51] uncrust Signed-off-by: Patrick Roncagliolo --- .vscode/launch.json | 34 ++++++++++ .vscode/settings.json | 75 +++++++++++++++++++++ rosbag2_storage/src/rosbag2_storage/qos.cpp | 4 +- 3 files changed, 111 insertions(+), 2 deletions(-) create mode 100644 .vscode/launch.json create mode 100644 .vscode/settings.json diff --git a/.vscode/launch.json b/.vscode/launch.json new file mode 100644 index 0000000000..3c63249f26 --- /dev/null +++ b/.vscode/launch.json @@ -0,0 +1,34 @@ +{ + // Use IntelliSense to learn about possible attributes. + // Hover to view descriptions of existing attributes. + // For more information, visit: https://go.microsoft.com/fwlink/?linkid=830387 + "version": "0.2.0", + "configurations": [ + + { + "name": "(gdb) Launch", + "type": "cppdbg", + "request": "launch", + "program": "/usr/bin/python3", + "args": ["/home/roxy/ros2_rolling/install/ros2cli/bin/ros2" "bag", "info", "/home/roxy/ros2_rolling/src/ros2/rosbag2/ros2bag/test/resources/empty_bag"], + "stopAtEntry": false, + "cwd": "${fileDirname}", + "environment": [], + "externalConsole": false, + "MIMode": "gdb", + "setupCommands": [ + { + "description": "Enable pretty-printing for gdb", + "text": "-enable-pretty-printing", + "ignoreFailures": true + }, + { + "description": "Set Disassembly Flavor to Intel", + "text": "-gdb-set disassembly-flavor intel", + "ignoreFailures": true + } + ] + } + + ] +} \ No newline at end of file diff --git a/.vscode/settings.json b/.vscode/settings.json new file mode 100644 index 0000000000..8f357b3346 --- /dev/null +++ b/.vscode/settings.json @@ -0,0 +1,75 @@ +{ + "files.associations": { + "cctype": "cpp", + "clocale": "cpp", + "cmath": "cpp", + "csignal": "cpp", + "cstdarg": "cpp", + "cstddef": "cpp", + "cstdio": "cpp", + "cstdlib": "cpp", + "cstring": "cpp", + "ctime": "cpp", + "cwchar": "cpp", + "cwctype": "cpp", + "array": "cpp", + "atomic": "cpp", + "hash_map": "cpp", + "bit": "cpp", + "*.tcc": "cpp", + "bitset": "cpp", + "chrono": "cpp", + "compare": "cpp", + "complex": "cpp", + "concepts": "cpp", + "condition_variable": "cpp", + "cstdint": "cpp", + "deque": "cpp", + "forward_list": "cpp", + "list": "cpp", + "map": "cpp", + "set": "cpp", + "string": "cpp", + "unordered_map": "cpp", + "unordered_set": "cpp", + "vector": "cpp", + "exception": "cpp", + "algorithm": "cpp", + "functional": "cpp", + "iterator": "cpp", + "memory": "cpp", + "memory_resource": "cpp", + "numeric": "cpp", + "optional": "cpp", + "random": "cpp", + "ratio": "cpp", + "string_view": "cpp", + "system_error": "cpp", + "tuple": "cpp", + "type_traits": "cpp", + "utility": "cpp", + "fstream": "cpp", + "initializer_list": "cpp", + "iomanip": "cpp", + "iosfwd": "cpp", + "iostream": "cpp", + "istream": "cpp", + "limits": "cpp", + "mutex": "cpp", + "new": "cpp", + "numbers": "cpp", + "ostream": "cpp", + "semaphore": "cpp", + "shared_mutex": "cpp", + "sstream": "cpp", + "stdexcept": "cpp", + "stop_token": "cpp", + "streambuf": "cpp", + "thread": "cpp", + "cinttypes": "cpp", + "typeindex": "cpp", + "typeinfo": "cpp", + "any": "cpp", + "variant": "cpp" + } +} \ No newline at end of file diff --git a/rosbag2_storage/src/rosbag2_storage/qos.cpp b/rosbag2_storage/src/rosbag2_storage/qos.cpp index 5504205785..8cd2c24e35 100644 --- a/rosbag2_storage/src/rosbag2_storage/qos.cpp +++ b/rosbag2_storage/src/rosbag2_storage/qos.cpp @@ -162,7 +162,7 @@ bool convert::decode( auto reliability = static_cast(node["reliability"].as()); auto durability = static_cast(node["durability"].as()); auto liveliness = static_cast(node["liveliness"].as()); - switch(history) { + switch (history) { case RMW_QOS_POLICY_HISTORY_KEEP_LAST: qos.keep_last(node["depth"].as()); break; @@ -178,7 +178,7 @@ bool convert::decode( .liveliness(liveliness); } else { auto history = node["history"].as(); - switch(history) { + switch (history) { case RMW_QOS_POLICY_HISTORY_KEEP_LAST: qos.keep_last(node["depth"].as()); break; From ded8bf3d5d871839c160211f51587678c16695f0 Mon Sep 17 00:00:00 2001 From: Patrick Roncagliolo Date: Mon, 30 Oct 2023 09:19:01 +0100 Subject: [PATCH 35/51] Remove VScode files Signed-off-by: Patrick Roncagliolo --- .vscode/launch.json | 34 -------------------- .vscode/settings.json | 75 ------------------------------------------- 2 files changed, 109 deletions(-) delete mode 100644 .vscode/launch.json delete mode 100644 .vscode/settings.json diff --git a/.vscode/launch.json b/.vscode/launch.json deleted file mode 100644 index 3c63249f26..0000000000 --- a/.vscode/launch.json +++ /dev/null @@ -1,34 +0,0 @@ -{ - // Use IntelliSense to learn about possible attributes. - // Hover to view descriptions of existing attributes. - // For more information, visit: https://go.microsoft.com/fwlink/?linkid=830387 - "version": "0.2.0", - "configurations": [ - - { - "name": "(gdb) Launch", - "type": "cppdbg", - "request": "launch", - "program": "/usr/bin/python3", - "args": ["/home/roxy/ros2_rolling/install/ros2cli/bin/ros2" "bag", "info", "/home/roxy/ros2_rolling/src/ros2/rosbag2/ros2bag/test/resources/empty_bag"], - "stopAtEntry": false, - "cwd": "${fileDirname}", - "environment": [], - "externalConsole": false, - "MIMode": "gdb", - "setupCommands": [ - { - "description": "Enable pretty-printing for gdb", - "text": "-enable-pretty-printing", - "ignoreFailures": true - }, - { - "description": "Set Disassembly Flavor to Intel", - "text": "-gdb-set disassembly-flavor intel", - "ignoreFailures": true - } - ] - } - - ] -} \ No newline at end of file diff --git a/.vscode/settings.json b/.vscode/settings.json deleted file mode 100644 index 8f357b3346..0000000000 --- a/.vscode/settings.json +++ /dev/null @@ -1,75 +0,0 @@ -{ - "files.associations": { - "cctype": "cpp", - "clocale": "cpp", - "cmath": "cpp", - "csignal": "cpp", - "cstdarg": "cpp", - "cstddef": "cpp", - "cstdio": "cpp", - "cstdlib": "cpp", - "cstring": "cpp", - "ctime": "cpp", - "cwchar": "cpp", - "cwctype": "cpp", - "array": "cpp", - "atomic": "cpp", - "hash_map": "cpp", - "bit": "cpp", - "*.tcc": "cpp", - "bitset": "cpp", - "chrono": "cpp", - "compare": "cpp", - "complex": "cpp", - "concepts": "cpp", - "condition_variable": "cpp", - "cstdint": "cpp", - "deque": "cpp", - "forward_list": "cpp", - "list": "cpp", - "map": "cpp", - "set": "cpp", - "string": "cpp", - "unordered_map": "cpp", - "unordered_set": "cpp", - "vector": "cpp", - "exception": "cpp", - "algorithm": "cpp", - "functional": "cpp", - "iterator": "cpp", - "memory": "cpp", - "memory_resource": "cpp", - "numeric": "cpp", - "optional": "cpp", - "random": "cpp", - "ratio": "cpp", - "string_view": "cpp", - "system_error": "cpp", - "tuple": "cpp", - "type_traits": "cpp", - "utility": "cpp", - "fstream": "cpp", - "initializer_list": "cpp", - "iomanip": "cpp", - "iosfwd": "cpp", - "iostream": "cpp", - "istream": "cpp", - "limits": "cpp", - "mutex": "cpp", - "new": "cpp", - "numbers": "cpp", - "ostream": "cpp", - "semaphore": "cpp", - "shared_mutex": "cpp", - "sstream": "cpp", - "stdexcept": "cpp", - "stop_token": "cpp", - "streambuf": "cpp", - "thread": "cpp", - "cinttypes": "cpp", - "typeindex": "cpp", - "typeinfo": "cpp", - "any": "cpp", - "variant": "cpp" - } -} \ No newline at end of file From 7f9885e421c3990a899a904ef100b1441d852613 Mon Sep 17 00:00:00 2001 From: Patrick Roncagliolo Date: Mon, 30 Oct 2023 10:16:12 +0100 Subject: [PATCH 36/51] PR Diff analysis & reduction Signed-off-by: Patrick Roncagliolo --- .../include/rosbag2_storage/qos.hpp | 48 +------- .../include/rosbag2_storage/yaml.hpp | 3 +- rosbag2_storage/src/rosbag2_storage/qos.cpp | 114 ++++++++++++------ .../src/rosbag2_transport/player.cpp | 10 +- .../src/rosbag2_transport/recorder.cpp | 2 +- .../test/rosbag2_transport/test_qos.cpp | 58 +++------ 6 files changed, 110 insertions(+), 125 deletions(-) diff --git a/rosbag2_storage/include/rosbag2_storage/qos.hpp b/rosbag2_storage/include/rosbag2_storage/qos.hpp index 9793e0dd90..308f36f01c 100644 --- a/rosbag2_storage/include/rosbag2_storage/qos.hpp +++ b/rosbag2_storage/include/rosbag2_storage/qos.hpp @@ -141,57 +141,17 @@ struct ROSBAG2_STORAGE_PUBLIC convert template<> struct ROSBAG2_STORAGE_PUBLIC convert> { - static Node encode(const std::vector & rhs) - { - Node node{NodeType::Sequence}; - for (const auto & value : rhs) { - node.push_back(value); - } - return node; - } - + static Node encode(const std::vector & rhs); static bool decode( - const Node & node, std::vector & rhs, int version) - { - if (!node.IsSequence()) { - return false; - } - - rhs.clear(); - for (const auto & value : node) { - auto temp = decode_for_version(value, version); - rhs.push_back(temp); - } - return true; - } + const Node & node, std::vector & rhs, int version); }; template<> struct ROSBAG2_STORAGE_PUBLIC convert> { - static Node encode(const std::map & rhs) - { - Node node{NodeType::Sequence}; - for (const auto & [key, value] : rhs) { - node.force_insert(key, value); - } - return node; - } - + static Node encode(const std::map & rhs); static bool decode( - const Node & node, std::map & rhs, int version) - { - if (!node.IsMap()) { - return false; - } - - rhs.clear(); - for (const auto & element : node) { - auto temp = decode_for_version(element.second, version); - rhs[element.first.as()] = temp; - } - return true; - } + const Node & node, std::map & rhs, int version); }; } // namespace YAML diff --git a/rosbag2_storage/include/rosbag2_storage/yaml.hpp b/rosbag2_storage/include/rosbag2_storage/yaml.hpp index 636e7d0315..eb40a9d2b0 100644 --- a/rosbag2_storage/include/rosbag2_storage/yaml.hpp +++ b/rosbag2_storage/include/rosbag2_storage/yaml.hpp @@ -153,8 +153,7 @@ struct convert> rhs.clear(); for (const auto & value : node) { - auto temp = decode_for_version(value, version); - rhs.push_back(temp); + rhs.push_back(decode_for_version(value, version)); } return true; } diff --git a/rosbag2_storage/src/rosbag2_storage/qos.cpp b/rosbag2_storage/src/rosbag2_storage/qos.cpp index 8cd2c24e35..abd280590a 100644 --- a/rosbag2_storage/src/rosbag2_storage/qos.cpp +++ b/rosbag2_storage/src/rosbag2_storage/qos.cpp @@ -151,49 +151,93 @@ Node convert::encode( bool convert::decode( const Node & node, rosbag2_storage::Rosbag2QoS & 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; + + if (version <= 8) { + history = static_cast(node["history"].as()); + reliability = static_cast(node["reliability"].as()); + durability = static_cast(node["durability"].as()); + liveliness = static_cast(node["liveliness"].as()); + } else { + history = node["history"].as(); + reliability = node["reliability"].as(); + durability = node["durability"].as(); + liveliness = node["liveliness"].as(); + } + + switch (history) { + case RMW_QOS_POLICY_HISTORY_KEEP_LAST: + qos.keep_last(node["depth"].as()); + break; + case RMW_QOS_POLICY_HISTORY_KEEP_ALL: + qos.keep_all(); + break; + default: + 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()); - if (version <= 8) { - 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()); - switch (history) { - case RMW_QOS_POLICY_HISTORY_KEEP_LAST: - qos.keep_last(node["depth"].as()); - break; - case RMW_QOS_POLICY_HISTORY_KEEP_ALL: - qos.keep_all(); - break; - default: - qos.history(history); - } - qos - .reliability(reliability) - .durability(durability) - .liveliness(liveliness); - } else { - auto history = node["history"].as(); - switch (history) { - case RMW_QOS_POLICY_HISTORY_KEEP_LAST: - qos.keep_last(node["depth"].as()); - break; - case RMW_QOS_POLICY_HISTORY_KEEP_ALL: - qos.keep_all(); - break; - default: - qos.history(history); - } - qos - .reliability(node["reliability"].as()) - .durability(node["durability"].as()) - .liveliness(node["liveliness"].as()); + return true; +} + +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) { + auto temp = decode_for_version(value, version); + rhs.push_back(temp); + } + 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) { + auto temp = decode_for_version(element.second, version); + rhs[element.first.as()] = temp; + } return true; } } // namespace YAML diff --git a/rosbag2_transport/src/rosbag2_transport/player.cpp b/rosbag2_transport/src/rosbag2_transport/player.cpp index b0cb494817..add6b6aadc 100644 --- a/rosbag2_transport/src/rosbag2_transport/player.cpp +++ b/rosbag2_transport/src/rosbag2_transport/player.cpp @@ -74,18 +74,18 @@ rclcpp::QoS publisher_qos_for_topic( RCLCPP_INFO_STREAM( logger, "Overriding QoS profile for topic " << topic.name); - return rosbag2_storage::Rosbag2QoS{qos_it->second}; + return Rosbag2QoS{qos_it->second}; } else if (topic.offered_qos_profiles.empty()) { - return rosbag2_storage::Rosbag2QoS{}; + return Rosbag2QoS{}; } - std::vector casted; + std::vector casted; casted.reserve(topic.offered_qos_profiles.size()); std::transform( topic.offered_qos_profiles.begin(), topic.offered_qos_profiles.end(), std::back_inserter(casted), - [](auto & qos) {return static_cast(qos);}); - return rosbag2_storage::Rosbag2QoS::adapt_offer_to_recorded_offers(topic.name, casted); + [](auto & qos) {return static_cast(qos);}); + return Rosbag2QoS::adapt_offer_to_recorded_offers(topic.name, casted); } } // namespace diff --git a/rosbag2_transport/src/rosbag2_transport/recorder.cpp b/rosbag2_transport/src/rosbag2_transport/recorder.cpp index f47e1c64ce..c3baafeff3 100644 --- a/rosbag2_transport/src/rosbag2_transport/recorder.cpp +++ b/rosbag2_transport/src/rosbag2_transport/recorder.cpp @@ -103,7 +103,7 @@ class RecorderImpl * Find the QoS profile that should be used for subscribing. * * Uses the override from record_options, if it is specified for this topic. - * Otherwise, falls back to rosbag2_storage::Rosbag2QoS::adapt_request_to_offers + * Otherwise, falls back to Rosbag2QoS::adapt_request_to_offers * * \param topic_name The full name of the topic, with namespace (ex. /arm/joint_status). * \return The QoS profile to be used for subscribing. diff --git a/rosbag2_transport/test/rosbag2_transport/test_qos.cpp b/rosbag2_transport/test/rosbag2_transport/test_qos.cpp index fdc5e8a809..a71215fa78 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_qos.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_qos.cpp @@ -173,15 +173,13 @@ class AdaptiveQoSTest : public ::testing::Test const std::string topic_name_{"/topic"}; std::vector endpoints_{}; - const rosbag2_storage::Rosbag2QoS default_offer_{}; - const rosbag2_storage::Rosbag2QoS default_request_{}; + const Rosbag2QoS default_offer_{}; + const Rosbag2QoS default_request_{}; }; TEST_F(AdaptiveQoSTest, adapt_request_empty_returns_default) { - auto adapted_request = rosbag2_storage::Rosbag2QoS::adapt_request_to_offers( - topic_name_, - endpoints_); + auto adapted_request = Rosbag2QoS::adapt_request_to_offers(topic_name_, endpoints_); EXPECT_EQ(default_request_, adapted_request); } @@ -189,12 +187,10 @@ TEST_F(AdaptiveQoSTest, adapt_request_single_offer_returns_same_values) { // Set up this offer to use nondefault reliability and durability, // expect to see those values in the output - auto nondefault_offer = rosbag2_storage::Rosbag2QoS{}.best_effort().transient_local(); + auto nondefault_offer = Rosbag2QoS{}.best_effort().transient_local(); add_endpoint(nondefault_offer); - auto adapted_request = rosbag2_storage::Rosbag2QoS::adapt_request_to_offers( - topic_name_, - endpoints_); + auto adapted_request = Rosbag2QoS::adapt_request_to_offers(topic_name_, endpoints_); auto expected = nondefault_offer.get_rmw_qos_profile(); auto actual = adapted_request.get_rmw_qos_profile(); @@ -204,15 +200,13 @@ TEST_F(AdaptiveQoSTest, adapt_request_single_offer_returns_same_values) TEST_F(AdaptiveQoSTest, adapt_request_multiple_similar_offers_returns_same_values) { - auto nondefault_offer = rosbag2_storage::Rosbag2QoS{}.best_effort().transient_local(); + auto nondefault_offer = Rosbag2QoS{}.best_effort().transient_local(); const size_t num_endpoints{3}; for (size_t i = 0; i < num_endpoints; i++) { add_endpoint(nondefault_offer); } - auto adapted_request = rosbag2_storage::Rosbag2QoS::adapt_request_to_offers( - topic_name_, - endpoints_); + auto adapted_request = Rosbag2QoS::adapt_request_to_offers(topic_name_, endpoints_); auto expected = nondefault_offer.get_rmw_qos_profile(); auto actual = adapted_request.get_rmw_qos_profile(); @@ -224,9 +218,7 @@ TEST_F(AdaptiveQoSTest, adapt_request_mixed_reliability_offers_return_best_effor { add_endpoint(Rosbag2QoS{}.best_effort()); add_endpoint(Rosbag2QoS{}.reliable()); - auto adapted_request = rosbag2_storage::Rosbag2QoS::adapt_request_to_offers( - topic_name_, - endpoints_); + auto adapted_request = Rosbag2QoS::adapt_request_to_offers(topic_name_, endpoints_); EXPECT_EQ( adapted_request.get_rmw_qos_profile().reliability, RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT); } @@ -235,15 +227,13 @@ TEST_F(AdaptiveQoSTest, adapt_request_mixed_durability_offers_return_volatile) { add_endpoint(Rosbag2QoS{}.transient_local()); add_endpoint(Rosbag2QoS{}.durability_volatile()); - auto adapted_request = rosbag2_storage::Rosbag2QoS::adapt_request_to_offers( - topic_name_, - endpoints_); + auto adapted_request = Rosbag2QoS::adapt_request_to_offers(topic_name_, endpoints_); EXPECT_EQ(adapted_request.get_rmw_qos_profile().durability, RMW_QOS_POLICY_DURABILITY_VOLATILE); } TEST_F(AdaptiveQoSTest, adapt_offer_empty_returns_default) { - auto adapted_offer = rosbag2_storage::Rosbag2QoS::adapt_offer_to_recorded_offers(topic_name_, {}); + auto adapted_offer = Rosbag2QoS::adapt_offer_to_recorded_offers(topic_name_, {}); EXPECT_EQ(adapted_offer, default_offer_); } @@ -251,21 +241,17 @@ TEST_F(AdaptiveQoSTest, adapt_offer_single_offer_returns_same_values) { // Set up this offer to use nondefault reliability and durability, // expect to see those values in the output - auto nondefault_offer = - rosbag2_storage::Rosbag2QoS{rosbag2_storage::Rosbag2QoS{}.best_effort().transient_local()}; + auto nondefault_offer = Rosbag2QoS{Rosbag2QoS{}.best_effort().transient_local()}; std::vector offers = {nondefault_offer}; - auto adapted_offer = rosbag2_storage::Rosbag2QoS::adapt_offer_to_recorded_offers( - topic_name_, - offers); + auto adapted_offer = Rosbag2QoS::adapt_offer_to_recorded_offers(topic_name_, offers); EXPECT_EQ(nondefault_offer, adapted_offer); } TEST_F(AdaptiveQoSTest, adapt_offer_multiple_offers_with_same_settings_return_identical) { - auto nondefault_offer = - rosbag2_storage::Rosbag2QoS{rosbag2_storage::Rosbag2QoS{}.best_effort().transient_local()}; - auto adapted_offer = rosbag2_storage::Rosbag2QoS::adapt_offer_to_recorded_offers( + auto nondefault_offer = Rosbag2QoS{Rosbag2QoS{}.best_effort().transient_local()}; + auto adapted_offer = Rosbag2QoS::adapt_offer_to_recorded_offers( topic_name_, {nondefault_offer, nondefault_offer, nondefault_offer}); EXPECT_EQ(nondefault_offer, adapted_offer); } @@ -275,12 +261,10 @@ TEST_F(AdaptiveQoSTest, adapt_offer_mixed_compatibility_returns_default) // When the offers have mixed values for policies that affect compatibility, // it should fall back to the default. std::vector offers = { - rosbag2_storage::Rosbag2QoS{rosbag2_storage::Rosbag2QoS{}.best_effort()}, - rosbag2_storage::Rosbag2QoS{rosbag2_storage::Rosbag2QoS{}.reliable()}, + Rosbag2QoS{Rosbag2QoS{}.best_effort()}, + Rosbag2QoS{Rosbag2QoS{}.reliable()}, }; - auto adapted_offer = rosbag2_storage::Rosbag2QoS::adapt_offer_to_recorded_offers( - topic_name_, - offers); + auto adapted_offer = Rosbag2QoS::adapt_offer_to_recorded_offers(topic_name_, offers); EXPECT_EQ(adapted_offer, default_offer_); } @@ -291,11 +275,9 @@ TEST_F(AdaptiveQoSTest, adapt_offer_mixed_non_compatibility_returns_first) rclcpp::Duration nonstandard_duration(12, 34); size_t nonstandard_history{20}; std::vector offers = { - rosbag2_storage::Rosbag2QoS{rosbag2_storage::Rosbag2QoS{}.lifespan(nonstandard_duration)}, - rosbag2_storage::Rosbag2QoS{rosbag2_storage::Rosbag2QoS{}.keep_last(nonstandard_history)}, + Rosbag2QoS{Rosbag2QoS{}.lifespan(nonstandard_duration)}, + Rosbag2QoS{Rosbag2QoS{}.keep_last(nonstandard_history)}, }; - auto adapted_offer = rosbag2_storage::Rosbag2QoS::adapt_offer_to_recorded_offers( - topic_name_, - offers); + auto adapted_offer = Rosbag2QoS::adapt_offer_to_recorded_offers(topic_name_, offers); EXPECT_EQ(adapted_offer, offers[0]); } From 7b970a3b8b1ca86d91f9a3a7908d424a1abba012 Mon Sep 17 00:00:00 2001 From: Patrick Roncagliolo Date: Mon, 30 Oct 2023 12:22:37 +0100 Subject: [PATCH 37/51] Refactor conversion snippets Signed-off-by: Patrick Roncagliolo --- .../include/rosbag2_storage/qos.hpp | 6 +++ .../include/rosbag2_storage/yaml.hpp | 19 ++------ rosbag2_storage/src/rosbag2_storage/qos.cpp | 41 ++++++++++++++++ rosbag2_storage_mcap/src/mcap_storage.cpp | 23 ++------- .../sqlite_storage.cpp | 47 ++++--------------- .../src/rosbag2_transport/player.cpp | 10 ++-- .../test/rosbag2_transport/test_play.cpp | 28 +++++------ .../test/rosbag2_transport/test_record.cpp | 11 +---- 8 files changed, 82 insertions(+), 103 deletions(-) diff --git a/rosbag2_storage/include/rosbag2_storage/qos.hpp b/rosbag2_storage/include/rosbag2_storage/qos.hpp index 308f36f01c..54b69a60b2 100644 --- a/rosbag2_storage/include/rosbag2_storage/qos.hpp +++ b/rosbag2_storage/include/rosbag2_storage/qos.hpp @@ -68,6 +68,12 @@ class ROSBAG2_STORAGE_PUBLIC Rosbag2QoS : public rclcpp::QoS const std::string & topic_name, const std::vector & profiles); }; + +std::vector from_rclcpp_qos_vector(std::vector in); +std::string serialize_rclcpp_qos_vector(std::vector in); +std::vector to_rclcpp_qos_vector(std::vector in); +std::vector to_rclcpp_qos_vector(YAML::Node in, int version); +std::vector to_rclcpp_qos_vector(std::string serielized, int version); } // namespace rosbag2_storage namespace YAML diff --git a/rosbag2_storage/include/rosbag2_storage/yaml.hpp b/rosbag2_storage/include/rosbag2_storage/yaml.hpp index eb40a9d2b0..d78229ff9a 100644 --- a/rosbag2_storage/include/rosbag2_storage/yaml.hpp +++ b/rosbag2_storage/include/rosbag2_storage/yaml.hpp @@ -75,15 +75,8 @@ struct convert node["name"] = topic.name; node["type"] = topic.type; node["serialization_format"] = topic.serialization_format; - std::vector to_encode; - to_encode.reserve(topic.offered_qos_profiles.size()); - std::transform( - topic.offered_qos_profiles.begin(), topic.offered_qos_profiles.end(), - std::back_inserter(to_encode), - [](auto & qos) {return static_cast(qos);}); - auto temp = convert>::encode( - to_encode); - node["offered_qos_profiles"] = YAML::Dump(temp); + node["offered_qos_profiles"] = rosbag2_storage::serialize_rclcpp_qos_vector( + topic.offered_qos_profiles); node["type_description_hash"] = topic.type_description_hash; return node; } @@ -95,13 +88,7 @@ struct convert topic.serialization_format = node["serialization_format"].as(); if (version >= 4) { std::string qos_str = node["offered_qos_profiles"].as(); - if (qos_str != "") { - auto decoded = - decode_for_version>( - YAML::Load(qos_str), version); - topic.offered_qos_profiles.reserve(decoded.size()); - std::copy(decoded.begin(), decoded.end(), std::back_inserter(topic.offered_qos_profiles)); - } + 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(); diff --git a/rosbag2_storage/src/rosbag2_storage/qos.cpp b/rosbag2_storage/src/rosbag2_storage/qos.cpp index abd280590a..a48086e30b 100644 --- a/rosbag2_storage/src/rosbag2_storage/qos.cpp +++ b/rosbag2_storage/src/rosbag2_storage/qos.cpp @@ -365,4 +365,45 @@ Rosbag2QoS Rosbag2QoS::adapt_offer_to_recorded_offers( "Falling back to the rosbag2_storage default publisher offer."); return Rosbag2QoS{}; } + +std::vector from_rclcpp_qos_vector(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(std::vector in) +{ + std::vector to_encode = from_rclcpp_qos_vector(in); + auto node = YAML::convert>::encode(to_encode); + return YAML::Dump(node); +} + +std::vector to_rclcpp_qos_vector(std::vector in) +{ + std::vector out; + out.reserve(in.size()); + std::copy(in.begin(), in.end(), std::back_inserter(out)); + return out; +} + +std::vector to_rclcpp_qos_vector(YAML::Node node, int version) +{ + auto in = YAML::decode_for_version>( + node, + version); + return to_rclcpp_qos_vector(in); +} + +std::vector to_rclcpp_qos_vector(std::string serielized, int version) +{ + if (serielized == "") {return {};} + auto node = YAML::Load(serielized); + return to_rclcpp_qos_vector(node, version); +} + } // namespace rosbag2_storage diff --git a/rosbag2_storage_mcap/src/mcap_storage.cpp b/rosbag2_storage_mcap/src/mcap_storage.cpp index fb436d55f2..e330642dfe 100644 --- a/rosbag2_storage_mcap/src/mcap_storage.cpp +++ b/rosbag2_storage_mcap/src/mcap_storage.cpp @@ -438,14 +438,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()) { - if (metadata_it->second != "") { - auto node = YAML::Load(metadata_it->second); - auto decoded = YAML::decode_for_version>( - node, metadata_.version); - topic_info.topic_metadata.offered_qos_profiles.reserve(decoded.size()); - std::copy(decoded.begin(), decoded.end(), - std::back_inserter(topic_info.topic_metadata.offered_qos_profiles)); - } + 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()) { @@ -822,16 +816,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; - - std::vector to_encode; - to_encode.reserve(topic_info.topic_metadata.offered_qos_profiles.size()); - std::transform(topic_info.topic_metadata.offered_qos_profiles.begin(), - topic_info.topic_metadata.offered_qos_profiles.end(), - std::back_inserter(to_encode), [](auto & qos) { - return static_cast(qos); - }); - auto yaml_node = YAML::convert>::encode(to_encode); - channel.metadata.emplace("offered_qos_profiles", YAML::Dump(yaml_node)); + 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_sqlite3/src/rosbag2_storage_sqlite3/sqlite_storage.cpp b/rosbag2_storage_sqlite3/src/rosbag2_storage_sqlite3/sqlite_storage.cpp index d6e46e684f..a141ed6779 100644 --- a/rosbag2_storage_sqlite3/src/rosbag2_storage_sqlite3/sqlite_storage.cpp +++ b/rosbag2_storage_sqlite3/src/rosbag2_storage_sqlite3/sqlite_storage.cpp @@ -460,14 +460,6 @@ void SqliteStorage::create_topic( { std::lock_guard db_lock(database_write_mutex_); if (topics_.find(topic.name) == std::end(topics_)) { - std::vector to_encode; - to_encode.reserve(topic.offered_qos_profiles.size()); - std::transform( - topic.offered_qos_profiles.begin(), - topic.offered_qos_profiles.end(), - std::back_inserter(to_encode), - [](auto & qos) {return static_cast(qos);}); - auto yaml_node = YAML::convert>::encode(to_encode); auto insert_topic = database_->prepare_statement( "INSERT INTO topics" @@ -477,7 +469,7 @@ void SqliteStorage::create_topic( topic.name, topic.type, topic.serialization_format, - YAML::Dump(yaml_node), + 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())); @@ -599,13 +591,8 @@ void SqliteStorage::fill_topics_and_types() std::string, std::string, std::string, std::string, std::string>(); for (auto result : query_results) { - auto yaml_node = YAML::Load(std::get<3>(result)); - auto decoded = YAML::decode_for_version>( - yaml_node, - metadata_.version); - std::vector offered_qos_profiles; - offered_qos_profiles.reserve(decoded.size()); - std::copy(decoded.begin(), decoded.end(), std::back_inserter(offered_qos_profiles)); + auto offered_qos_profiles = rosbag2_storage::to_rclcpp_qos_vector( + std::get<3>(result), metadata_.version); all_topics_and_types_.push_back( { std::get<0>(result), @@ -621,12 +608,8 @@ void SqliteStorage::fill_topics_and_types() std::string, std::string, std::string, std::string>(); for (auto result : query_results) { - auto yaml_node = YAML::Load(std::get<3>(result)); - auto decoded = YAML::decode_for_version>( - yaml_node, - metadata_.version); - std::vector offered_qos_profiles; - std::copy(decoded.begin(), decoded.end(), std::back_inserter(offered_qos_profiles)); + auto offered_qos_profiles = rosbag2_storage::to_rclcpp_qos_vector( + std::get<3>(result), metadata_.version); all_topics_and_types_.push_back( {std::get<0>(result), std::get<1>(result), std::get<2>(result), offered_qos_profiles, ""}); @@ -703,12 +686,8 @@ void SqliteStorage::read_metadata() rcutils_time_point_value_t, std::string, std::string>(); for (auto result : query_results) { - auto yaml_node = YAML::Load(std::get<6>(result)); - auto decoded = YAML::decode_for_version>( - yaml_node, - metadata_.version); - std::vector offered_qos_profiles; - std::copy(decoded.begin(), decoded.end(), std::back_inserter(offered_qos_profiles)); + auto offered_qos_profiles = rosbag2_storage::to_rclcpp_qos_vector( + std::get<6>(result), metadata_.version); metadata_.topics_with_message_count.push_back( { {std::get<0>(result), std::get<1>(result), std::get<2>( @@ -733,16 +712,8 @@ void SqliteStorage::read_metadata() rcutils_time_point_value_t, std::string>(); for (auto result : query_results) { - std::vector offered_qos_profiles {}; - std::string yaml_str = std::get<6>(result); - if (yaml_str != "") { - auto yaml_node = YAML::Load(yaml_str); - auto decoded = YAML::decode_for_version>( - yaml_node, - metadata_.version); - offered_qos_profiles.reserve(decoded.size()); - std::copy(decoded.begin(), decoded.end(), std::back_inserter(offered_qos_profiles)); - } + auto offered_qos_profiles = rosbag2_storage::to_rclcpp_qos_vector( + std::get<6>(result), metadata_.version); metadata_.topics_with_message_count.push_back( { {std::get<0>(result), std::get<1>(result), std::get<2>( diff --git a/rosbag2_transport/src/rosbag2_transport/player.cpp b/rosbag2_transport/src/rosbag2_transport/player.cpp index add6b6aadc..8c37e420e3 100644 --- a/rosbag2_transport/src/rosbag2_transport/player.cpp +++ b/rosbag2_transport/src/rosbag2_transport/player.cpp @@ -79,13 +79,9 @@ rclcpp::QoS publisher_qos_for_topic( return Rosbag2QoS{}; } - std::vector casted; - casted.reserve(topic.offered_qos_profiles.size()); - std::transform( - topic.offered_qos_profiles.begin(), topic.offered_qos_profiles.end(), - std::back_inserter(casted), - [](auto & qos) {return static_cast(qos);}); - return Rosbag2QoS::adapt_offer_to_recorded_offers(topic.name, casted); + 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/test/rosbag2_transport/test_play.cpp b/rosbag2_transport/test/rosbag2_transport/test_play.cpp index cf9b73c6ee..db60070a07 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_play.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_play.cpp @@ -467,11 +467,12 @@ class RosBag2PlayQosOverrideTestFixture : public RosBag2PlayTestFixture messages_.push_back(serialize_test_message(topic_name_, timestamp, basic_msg_)); } - std::vector casted; - casted.reserve(offered_qos.size()); - std::copy(offered_qos.begin(), offered_qos.end(), std::back_inserter(casted)); - - topic_types_.push_back({topic_name_, msg_type_, "", casted, ""}); + topic_types_.push_back({ + topic_name_, + msg_type_, + "", + rosbag2_storage::to_rclcpp_qos_vector(offered_qos), + ""}); } template @@ -508,6 +509,7 @@ class RosBag2PlayQosOverrideTestFixture : public RosBag2PlayTestFixture std::vector> messages_; }; +using rosbag2_storage::Rosbag2QoS; TEST_F(RosBag2PlayQosOverrideTestFixture, topic_qos_profiles_overridden) { // By default playback uses DURABILITY_VOLATILE. @@ -562,8 +564,7 @@ TEST_F(RosBag2PlayQosOverrideTestFixture, playback_uses_recorded_transient_local // In this test, we subscribe requesting DURABILITY_TRANSIENT_LOCAL. // The bag metadata has this recorded for the original Publisher, // so playback's offer should be compatible (whereas the default offer would not be) - const auto transient_local_profile = - rosbag2_storage::Rosbag2QoS{rosbag2_storage::Rosbag2QoS{}.transient_local()}; + const auto transient_local_profile = Rosbag2QoS{Rosbag2QoS{}.transient_local()}; // This should normally take less than 1s - just making it shorter than 60s default const auto timeout = 5s; @@ -586,9 +587,8 @@ TEST_F(RosBag2PlayQosOverrideTestFixture, playback_uses_recorded_deadline) // requirement, so they are compatible. const rclcpp::Duration request_deadline{1s}; const rclcpp::Duration offer_deadline{500ms}; - const auto request_profile = rosbag2_storage::Rosbag2QoS{}.deadline(request_deadline); - const auto offer_profile = rosbag2_storage::Rosbag2QoS{rosbag2_storage::Rosbag2QoS{}.deadline( - offer_deadline)}; + const auto request_profile = Rosbag2QoS{}.deadline(request_deadline); + const auto offer_profile = Rosbag2QoS{Rosbag2QoS{}.deadline(offer_deadline)}; const auto timeout = 5s; initialize({offer_profile}); @@ -609,13 +609,11 @@ TEST_F(RosBag2PlayQosOverrideTestFixture, override_has_precedence_over_recorded) const rclcpp::Duration override_liveliness_offer{250ms}; ASSERT_LT(liveliness_request, recorded_liveliness_offer); ASSERT_LT(override_liveliness_offer, liveliness_request); - const auto request_profile = rosbag2_storage::Rosbag2QoS{}.liveliness_lease_duration( + const auto request_profile = Rosbag2QoS{}.liveliness_lease_duration( liveliness_request); - const auto recorded_offer_profile = - rosbag2_storage::Rosbag2QoS{rosbag2_storage::Rosbag2QoS{}.liveliness_lease_duration( + const auto recorded_offer_profile = Rosbag2QoS{Rosbag2QoS{}.liveliness_lease_duration( recorded_liveliness_offer)}; - const auto override_offer_profile = - rosbag2_storage::Rosbag2QoS{rosbag2_storage::Rosbag2QoS{}.liveliness_lease_duration( + const auto override_offer_profile = Rosbag2QoS{Rosbag2QoS{}.liveliness_lease_duration( override_liveliness_offer)}; const auto topic_qos_profile_overrides = std::unordered_map{ std::pair{topic_name_, override_offer_profile}, diff --git a/rosbag2_transport/test/rosbag2_transport/test_record.cpp b/rosbag2_transport/test/rosbag2_transport/test_record.cpp index 4bfcd9a405..48e0d95516 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_record.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_record.cpp @@ -176,15 +176,8 @@ TEST_F(RecordIntegrationTestFixture, qos_is_stored_in_metadata) auto recorded_topics = mock_writer.get_topics(); auto offered_qos_profiles = recorded_topics.at(topic).first.offered_qos_profiles; - - - std::vector to_encode; - to_encode.reserve(offered_qos_profiles.size()); - std::transform( - offered_qos_profiles.begin(), offered_qos_profiles.end(), std::back_inserter(to_encode), - [](auto & qos) {return static_cast(qos);}); - auto temp = YAML::convert>::encode(to_encode); - std::string serialized_profiles = YAML::Dump(temp); + 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: reliable\n")); EXPECT_THAT(serialized_profiles, ContainsRegex("durability: volatile\n")); From fe67da886d383448e2d27d46e8c2f5fdffe25ed5 Mon Sep 17 00:00:00 2001 From: Patrick Roncagliolo Date: Mon, 30 Oct 2023 12:27:50 +0100 Subject: [PATCH 38/51] Uncrustify Signed-off-by: Patrick Roncagliolo --- rosbag2_transport/test/rosbag2_transport/test_play.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/rosbag2_transport/test/rosbag2_transport/test_play.cpp b/rosbag2_transport/test/rosbag2_transport/test_play.cpp index db60070a07..9c9618791b 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_play.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_play.cpp @@ -467,7 +467,8 @@ class RosBag2PlayQosOverrideTestFixture : public RosBag2PlayTestFixture messages_.push_back(serialize_test_message(topic_name_, timestamp, basic_msg_)); } - topic_types_.push_back({ + topic_types_.push_back( + { topic_name_, msg_type_, "", From 6cd68a2f8de7a6dca5fb9785a82ab32c844bcf39 Mon Sep 17 00:00:00 2001 From: Patrick Roncagliolo Date: Thu, 2 Nov 2023 09:50:45 +0100 Subject: [PATCH 39/51] Address some comments Signed-off-by: Patrick Roncagliolo --- rosbag2_storage/CMakeLists.txt | 4 +++- .../include/rosbag2_storage/qos.hpp | 11 ++++++----- rosbag2_storage/package.xml | 1 + rosbag2_storage/src/rosbag2_storage/qos.cpp | 18 +++++++++--------- .../rosbag2_storage_sqlite3/sqlite_storage.cpp | 16 ++++++++++++---- .../rosbag2_transport/record_options.hpp | 3 +-- .../test/rosbag2_transport/test_play.cpp | 3 +-- 7 files changed, 33 insertions(+), 23 deletions(-) diff --git a/rosbag2_storage/CMakeLists.txt b/rosbag2_storage/CMakeLists.txt index 66f7d0a65d..4b9318f22a 100644 --- a/rosbag2_storage/CMakeLists.txt +++ b/rosbag2_storage/CMakeLists.txt @@ -26,6 +26,7 @@ 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( @@ -48,6 +49,7 @@ target_link_libraries(${PROJECT_NAME} rcpputils::rcpputils rcutils::rcutils rclcpp::rclcpp + rmw::rmw yaml-cpp ) @@ -73,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 rclcpp) +ament_export_dependencies(pluginlib yaml_cpp_vendor rclcpp rmw) if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) diff --git a/rosbag2_storage/include/rosbag2_storage/qos.hpp b/rosbag2_storage/include/rosbag2_storage/qos.hpp index 54b69a60b2..ce3a751014 100644 --- a/rosbag2_storage/include/rosbag2_storage/qos.hpp +++ b/rosbag2_storage/include/rosbag2_storage/qos.hpp @@ -69,11 +69,12 @@ class ROSBAG2_STORAGE_PUBLIC Rosbag2QoS : public rclcpp::QoS const std::vector & profiles); }; -std::vector from_rclcpp_qos_vector(std::vector in); -std::string serialize_rclcpp_qos_vector(std::vector in); -std::vector to_rclcpp_qos_vector(std::vector in); -std::vector to_rclcpp_qos_vector(YAML::Node in, int version); -std::vector to_rclcpp_qos_vector(std::string serielized, int version); +std::vector from_rclcpp_qos_vector( + const std::vector & in); +std::string serialize_rclcpp_qos_vector(const std::vector & in); +std::vector to_rclcpp_qos_vector(const std::vector & in); +std::vector to_rclcpp_qos_vector(const YAML::Node & in, int version); +std::vector to_rclcpp_qos_vector(const std::string & serialized, int version); } // namespace rosbag2_storage namespace YAML diff --git a/rosbag2_storage/package.xml b/rosbag2_storage/package.xml index 81242ce1fa..3d5889074d 100644 --- a/rosbag2_storage/package.xml +++ b/rosbag2_storage/package.xml @@ -16,6 +16,7 @@ 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 index a48086e30b..61342aff9a 100644 --- a/rosbag2_storage/src/rosbag2_storage/qos.cpp +++ b/rosbag2_storage/src/rosbag2_storage/qos.cpp @@ -235,8 +235,8 @@ bool convert>::decode( rhs.clear(); for (const auto & element : node) { - auto temp = decode_for_version(element.second, version); - rhs[element.first.as()] = temp; + rhs[element.first.as()] = decode_for_version( + element.second, version); } return true; } @@ -366,7 +366,7 @@ Rosbag2QoS Rosbag2QoS::adapt_offer_to_recorded_offers( return Rosbag2QoS{}; } -std::vector from_rclcpp_qos_vector(std::vector in) +std::vector from_rclcpp_qos_vector(const std::vector & in) { std::vector out; out.reserve(in.size()); @@ -376,14 +376,14 @@ std::vector from_rclcpp_qos_vector(std::vector in) +std::string serialize_rclcpp_qos_vector(const std::vector & in) { std::vector to_encode = from_rclcpp_qos_vector(in); auto node = YAML::convert>::encode(to_encode); return YAML::Dump(node); } -std::vector to_rclcpp_qos_vector(std::vector in) +std::vector to_rclcpp_qos_vector(const std::vector & in) { std::vector out; out.reserve(in.size()); @@ -391,7 +391,7 @@ std::vector to_rclcpp_qos_vector(std::vector to_rclcpp_qos_vector(YAML::Node node, int version) +std::vector to_rclcpp_qos_vector(const YAML::Node & node, int version) { auto in = YAML::decode_for_version>( node, @@ -399,10 +399,10 @@ std::vector to_rclcpp_qos_vector(YAML::Node node, int version) return to_rclcpp_qos_vector(in); } -std::vector to_rclcpp_qos_vector(std::string serielized, int version) +std::vector to_rclcpp_qos_vector(const std::string & serialized, int version) { - if (serielized == "") {return {};} - auto node = YAML::Load(serielized); + if (serialized == "") {return {};} + auto node = YAML::Load(serialized); return to_rclcpp_qos_vector(node, version); } 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 a141ed6779..aa573e9856 100644 --- a/rosbag2_storage_sqlite3/src/rosbag2_storage_sqlite3/sqlite_storage.cpp +++ b/rosbag2_storage_sqlite3/src/rosbag2_storage_sqlite3/sqlite_storage.cpp @@ -592,7 +592,9 @@ void SqliteStorage::fill_topics_and_types() for (auto result : query_results) { auto offered_qos_profiles = rosbag2_storage::to_rclcpp_qos_vector( - std::get<3>(result), metadata_.version); + // 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), @@ -609,7 +611,9 @@ void SqliteStorage::fill_topics_and_types() for (auto result : query_results) { auto offered_qos_profiles = rosbag2_storage::to_rclcpp_qos_vector( - std::get<3>(result), metadata_.version); + // 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), offered_qos_profiles, ""}); @@ -687,7 +691,9 @@ void SqliteStorage::read_metadata() for (auto result : query_results) { auto offered_qos_profiles = rosbag2_storage::to_rclcpp_qos_vector( - std::get<6>(result), metadata_.version); + // 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>( @@ -713,7 +719,9 @@ void SqliteStorage::read_metadata() for (auto result : query_results) { auto offered_qos_profiles = rosbag2_storage::to_rclcpp_qos_vector( - std::get<6>(result), metadata_.version); + // 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>( diff --git a/rosbag2_transport/include/rosbag2_transport/record_options.hpp b/rosbag2_transport/include/rosbag2_transport/record_options.hpp index d67ba45cf4..2ba15b3353 100644 --- a/rosbag2_transport/include/rosbag2_transport/record_options.hpp +++ b/rosbag2_transport/include/rosbag2_transport/record_options.hpp @@ -59,8 +59,7 @@ 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, - int version); + const Node & node, rosbag2_transport::RecordOptions & storage_options, int version); }; } // namespace YAML diff --git a/rosbag2_transport/test/rosbag2_transport/test_play.cpp b/rosbag2_transport/test/rosbag2_transport/test_play.cpp index 9c9618791b..931e1b4d5a 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_play.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_play.cpp @@ -610,8 +610,7 @@ TEST_F(RosBag2PlayQosOverrideTestFixture, override_has_precedence_over_recorded) const rclcpp::Duration override_liveliness_offer{250ms}; ASSERT_LT(liveliness_request, recorded_liveliness_offer); ASSERT_LT(override_liveliness_offer, liveliness_request); - const auto request_profile = Rosbag2QoS{}.liveliness_lease_duration( - liveliness_request); + const auto request_profile = Rosbag2QoS{}.liveliness_lease_duration(liveliness_request); const auto recorded_offer_profile = Rosbag2QoS{Rosbag2QoS{}.liveliness_lease_duration( recorded_liveliness_offer)}; const auto override_offer_profile = Rosbag2QoS{Rosbag2QoS{}.liveliness_lease_duration( From d8243dd1f1967b6240ef00de186145c1fc07da6f Mon Sep 17 00:00:00 2001 From: Patrick Roncagliolo Date: Thu, 2 Nov 2023 12:29:01 +0100 Subject: [PATCH 40/51] Address some comments Signed-off-by: Patrick Roncagliolo --- .../include/rosbag2_storage/qos.hpp | 20 +++++++---- rosbag2_storage/src/rosbag2_storage/qos.cpp | 34 ++++++++++++++----- .../sqlite_storage.cpp | 24 ++++++------- .../test/rosbag2_transport/test_qos.cpp | 3 +- 4 files changed, 52 insertions(+), 29 deletions(-) diff --git a/rosbag2_storage/include/rosbag2_storage/qos.hpp b/rosbag2_storage/include/rosbag2_storage/qos.hpp index ce3a751014..b25b66b99c 100644 --- a/rosbag2_storage/include/rosbag2_storage/qos.hpp +++ b/rosbag2_storage/include/rosbag2_storage/qos.hpp @@ -103,31 +103,37 @@ T decode_for_version(const Node & node, int version) throw TypedBadConversion(node.Mark()); } +template +Node encode_for_version(const T & value, int version) +{ + return convert::encode(value, version); +} + template<> struct ROSBAG2_STORAGE_PUBLIC convert { - static Node encode(const rmw_qos_history_policy_t & policy); + 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); + 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); + 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); + static Node encode(const rmw_qos_liveliness_policy_t & policy, int version); static bool decode(const Node & node, rmw_qos_liveliness_policy_t & policy); }; @@ -142,7 +148,7 @@ template<> struct ROSBAG2_STORAGE_PUBLIC convert { static Node encode(const rosbag2_storage::Rosbag2QoS & qos); - static bool decode(const Node & node, rosbag2_storage::Rosbag2QoS & qos, int version); + static bool decode(const Node & node, rosbag2_storage::Rosbag2QoS & qos, int version = 9); }; template<> @@ -150,7 +156,7 @@ struct ROSBAG2_STORAGE_PUBLIC convert> { static Node encode(const std::vector & rhs); static bool decode( - const Node & node, std::vector & rhs, int version); + const Node & node, std::vector & rhs, int version = 9); }; template<> @@ -158,7 +164,7 @@ struct ROSBAG2_STORAGE_PUBLIC convert & rhs); static bool decode( - const Node & node, std::map & rhs, int version); + const Node & node, std::map & rhs, int version = 9); }; } // namespace YAML diff --git a/rosbag2_storage/src/rosbag2_storage/qos.cpp b/rosbag2_storage/src/rosbag2_storage/qos.cpp index 61342aff9a..70f0a0f916 100644 --- a/rosbag2_storage/src/rosbag2_storage/qos.cpp +++ b/rosbag2_storage/src/rosbag2_storage/qos.cpp @@ -43,8 +43,11 @@ static const rmw_time_t RMW_CONNEXT_FOXY_INFINITE {0x7FFFFFFFll, 0x7FFFFFFFll}; namespace YAML { -Node convert::encode(const rmw_qos_history_policy_t & policy) +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 { @@ -58,8 +61,13 @@ bool convert::decode(const Node & node, rmw_qos_histor return true; } -Node convert::encode(const rmw_qos_reliability_policy_t & policy) +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 { @@ -75,8 +83,13 @@ bool convert::decode( return true; } -Node convert::encode(const rmw_qos_durability_policy_t & policy) +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 { @@ -92,8 +105,13 @@ bool convert::decode( return true; } -Node convert::encode(const rmw_qos_liveliness_policy_t & policy) +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 { @@ -136,13 +154,13 @@ Node convert::encode( { const auto & p = qos.get_rmw_qos_profile(); Node node; - node["history"] = convert::encode(p.history); + node["history"] = convert::encode(p.history, 9); node["depth"] = p.depth; - node["reliability"] = convert::encode(p.reliability); - node["durability"] = convert::encode(p.durability); + node["reliability"] = convert::encode(p.reliability, 9); + node["durability"] = convert::encode(p.durability, 9); node["deadline"] = p.deadline; node["lifespan"] = p.lifespan; - node["liveliness"] = convert::encode(p.liveliness); + node["liveliness"] = convert::encode(p.liveliness, 9); node["liveliness_lease_duration"] = p.liveliness_lease_duration; node["avoid_ros_namespace_conventions"] = p.avoid_ros_namespace_conventions; return node; 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 aa573e9856..a1e32ebee4 100644 --- a/rosbag2_storage_sqlite3/src/rosbag2_storage_sqlite3/sqlite_storage.cpp +++ b/rosbag2_storage_sqlite3/src/rosbag2_storage_sqlite3/sqlite_storage.cpp @@ -592,9 +592,9 @@ void SqliteStorage::fill_topics_and_types() 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); + // 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), @@ -611,9 +611,9 @@ void SqliteStorage::fill_topics_and_types() 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); + // 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), offered_qos_profiles, ""}); @@ -691,9 +691,9 @@ void SqliteStorage::read_metadata() 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); + // 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>( @@ -719,9 +719,9 @@ void SqliteStorage::read_metadata() 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); + // 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>( diff --git a/rosbag2_transport/test/rosbag2_transport/test_qos.cpp b/rosbag2_transport/test/rosbag2_transport/test_qos.cpp index a71215fa78..857f3c40c8 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_qos.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_qos.cpp @@ -136,8 +136,7 @@ TEST(TestQoS, translates_bad_infinity_values) " nsec: " << infinity.nsec << "\n" "avoid_ros_namespace_conventions: false\n"; const YAML::Node loaded_node = YAML::Load(serialized_profile.str()); - const auto deserialized_profile = YAML::decode_for_version( - loaded_node, 9); + 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)); From a3ffbc43ae7c36cf0813892ae296252e1ff7a7b4 Mon Sep 17 00:00:00 2001 From: Patrick Roncagliolo Date: Thu, 2 Nov 2023 14:15:33 +0100 Subject: [PATCH 41/51] Address some comments Signed-off-by: Patrick Roncagliolo --- rosbag2_transport/include/rosbag2_transport/record_options.hpp | 2 +- rosbag2_transport/test/rosbag2_transport/test_play.cpp | 3 ++- rosbag2_transport/test/rosbag2_transport/test_qos.cpp | 2 +- .../test/rosbag2_transport/test_record_options.cpp | 3 +-- 4 files changed, 5 insertions(+), 5 deletions(-) diff --git a/rosbag2_transport/include/rosbag2_transport/record_options.hpp b/rosbag2_transport/include/rosbag2_transport/record_options.hpp index 2ba15b3353..50621877f8 100644 --- a/rosbag2_transport/include/rosbag2_transport/record_options.hpp +++ b/rosbag2_transport/include/rosbag2_transport/record_options.hpp @@ -59,7 +59,7 @@ 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, int version); + const Node & node, rosbag2_transport::RecordOptions & storage_options, int version = 9); }; } // namespace YAML diff --git a/rosbag2_transport/test/rosbag2_transport/test_play.cpp b/rosbag2_transport/test/rosbag2_transport/test_play.cpp index 931e1b4d5a..adc9970a15 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_play.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_play.cpp @@ -450,6 +450,8 @@ TEST_F(RosBag2PlayTestFixture, player_gracefully_exit_by_rclcpp_shutdown_in_paus class RosBag2PlayQosOverrideTestFixture : public RosBag2PlayTestFixture { public: + using Rosbag2QoS = rosbag2_storage::Rosbag2QoS; + RosBag2PlayQosOverrideTestFixture() : RosBag2PlayTestFixture() { @@ -510,7 +512,6 @@ class RosBag2PlayQosOverrideTestFixture : public RosBag2PlayTestFixture std::vector> messages_; }; -using rosbag2_storage::Rosbag2QoS; TEST_F(RosBag2PlayQosOverrideTestFixture, topic_qos_profiles_overridden) { // By default playback uses DURABILITY_VOLATILE. diff --git a/rosbag2_transport/test/rosbag2_transport/test_qos.cpp b/rosbag2_transport/test/rosbag2_transport/test_qos.cpp index 857f3c40c8..d8313bba75 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_qos.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_qos.cpp @@ -146,10 +146,10 @@ TEST(TestQoS, translates_bad_infinity_values) } } -using rosbag2_storage::Rosbag2QoS; // NOLINT class AdaptiveQoSTest : public ::testing::Test { public: + using Rosbag2QoS = rosbag2_storage::Rosbag2QoS; AdaptiveQoSTest() = default; rclcpp::TopicEndpointInfo make_endpoint(const rclcpp::QoS & qos) diff --git a/rosbag2_transport/test/rosbag2_transport/test_record_options.cpp b/rosbag2_transport/test/rosbag2_transport/test_record_options.cpp index 1bdfa21fef..e305229152 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_record_options.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_record_options.cpp @@ -42,8 +42,7 @@ TEST(record_options, test_yaml_serialization) std::stringstream serializer; serializer << node; auto reconstructed_node = YAML::Load(serializer.str()); - auto reconstructed = YAML::decode_for_version( - reconstructed_node, 9); + auto reconstructed = reconstructed_node.as(); #define CHECK(field) ASSERT_EQ(original.field, reconstructed.field) CHECK(all); From 01f25dd15179a4f31aff13dd031bdc8ec8f3e3f2 Mon Sep 17 00:00:00 2001 From: Patrick Roncagliolo Date: Thu, 2 Nov 2023 15:40:29 +0100 Subject: [PATCH 42/51] More fixes Signed-off-by: Patrick Roncagliolo --- rosbag2_storage/CMakeLists.txt | 10 +++- .../include/rosbag2_storage/qos.hpp | 10 +++- rosbag2_storage/src/rosbag2_storage/qos.cpp | 56 ++++++++++++------- .../test/rosbag2_storage}/test_qos.cpp | 0 rosbag2_transport/CMakeLists.txt | 6 -- .../test/rosbag2_transport/test_play.cpp | 6 +- 6 files changed, 55 insertions(+), 33 deletions(-) rename {rosbag2_transport/test/rosbag2_transport => rosbag2_storage/test/rosbag2_storage}/test_qos.cpp (100%) diff --git a/rosbag2_storage/CMakeLists.txt b/rosbag2_storage/CMakeLists.txt index 4b9318f22a..1fbd3e6dba 100644 --- a/rosbag2_storage/CMakeLists.txt +++ b/rosbag2_storage/CMakeLists.txt @@ -124,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/qos.hpp b/rosbag2_storage/include/rosbag2_storage/qos.hpp index b25b66b99c..b07a0a083e 100644 --- a/rosbag2_storage/include/rosbag2_storage/qos.hpp +++ b/rosbag2_storage/include/rosbag2_storage/qos.hpp @@ -72,8 +72,6 @@ class ROSBAG2_STORAGE_PUBLIC Rosbag2QoS : public rclcpp::QoS std::vector from_rclcpp_qos_vector( const std::vector & in); std::string serialize_rclcpp_qos_vector(const std::vector & in); -std::vector to_rclcpp_qos_vector(const std::vector & in); -std::vector to_rclcpp_qos_vector(const YAML::Node & in, int version); std::vector to_rclcpp_qos_vector(const std::string & serialized, int version); } // namespace rosbag2_storage @@ -159,6 +157,14 @@ struct ROSBAG2_STORAGE_PUBLIC convert> const Node & node, std::vector & rhs, 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> { diff --git a/rosbag2_storage/src/rosbag2_storage/qos.cpp b/rosbag2_storage/src/rosbag2_storage/qos.cpp index 70f0a0f916..c492883237 100644 --- a/rosbag2_storage/src/rosbag2_storage/qos.cpp +++ b/rosbag2_storage/src/rosbag2_storage/qos.cpp @@ -174,13 +174,18 @@ bool convert::decode( rmw_qos_durability_policy_t durability; rmw_qos_liveliness_policy_t liveliness; + int temp; + if (convert::decode(node["history"], temp)) { + history = static_cast(temp); + } else { + history = node["history"].as(); + } + if (version <= 8) { - history = static_cast(node["history"].as()); reliability = static_cast(node["reliability"].as()); durability = static_cast(node["durability"].as()); liveliness = static_cast(node["liveliness"].as()); } else { - history = node["history"].as(); reliability = node["reliability"].as(); durability = node["durability"].as(); liveliness = node["liveliness"].as(); @@ -234,6 +239,31 @@ bool convert>::decode( return true; } +Node convert>::encode( + const std::vector & rhs) +{ + Node node{NodeType::Sequence}; + for (const auto & value : rhs) { + node.push_back(static_cast(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) { + auto temp = decode_for_version(value, version); + rhs.push_back(temp); + } + return true; +} + Node convert>::encode( const std::map & rhs) { @@ -384,6 +414,7 @@ Rosbag2QoS Rosbag2QoS::adapt_offer_to_recorded_offers( return Rosbag2QoS{}; } + std::vector from_rclcpp_qos_vector(const std::vector & in) { std::vector out; @@ -396,32 +427,15 @@ std::vector from_rclcpp_qos_vector(const std::vecto std::string serialize_rclcpp_qos_vector(const std::vector & in) { - std::vector to_encode = from_rclcpp_qos_vector(in); - auto node = YAML::convert>::encode(to_encode); + auto node = YAML::convert>::encode(in); return YAML::Dump(node); } -std::vector to_rclcpp_qos_vector(const std::vector & in) -{ - std::vector out; - out.reserve(in.size()); - std::copy(in.begin(), in.end(), std::back_inserter(out)); - return out; -} - -std::vector to_rclcpp_qos_vector(const YAML::Node & node, int version) -{ - auto in = YAML::decode_for_version>( - node, - version); - return to_rclcpp_qos_vector(in); -} - std::vector to_rclcpp_qos_vector(const std::string & serialized, int version) { if (serialized == "") {return {};} auto node = YAML::Load(serialized); - return to_rclcpp_qos_vector(node, version); + return YAML::decode_for_version>(node, version); } } // namespace rosbag2_storage diff --git a/rosbag2_transport/test/rosbag2_transport/test_qos.cpp b/rosbag2_storage/test/rosbag2_storage/test_qos.cpp similarity index 100% rename from rosbag2_transport/test/rosbag2_transport/test_qos.cpp rename to rosbag2_storage/test/rosbag2_storage/test_qos.cpp diff --git a/rosbag2_transport/CMakeLists.txt b/rosbag2_transport/CMakeLists.txt index 0dc14ad27d..d3f66e6c80 100644 --- a/rosbag2_transport/CMakeLists.txt +++ b/rosbag2_transport/CMakeLists.txt @@ -177,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/test/rosbag2_transport/test_play.cpp b/rosbag2_transport/test/rosbag2_transport/test_play.cpp index adc9970a15..afda51a574 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_play.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_play.cpp @@ -451,13 +451,13 @@ 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 @@ -474,7 +474,7 @@ class RosBag2PlayQosOverrideTestFixture : public RosBag2PlayTestFixture topic_name_, msg_type_, "", - rosbag2_storage::to_rclcpp_qos_vector(offered_qos), + offered_qos, ""}); } From 6b4c2bd5e9aa6432613dd134781616e2ae584ebb Mon Sep 17 00:00:00 2001 From: Michael Orlov Date: Fri, 3 Nov 2023 14:00:44 -0700 Subject: [PATCH 43/51] Fixes for failing tests in the test_qos.cpp - Add Metadata version for encode methods to be able to encode rclcpp::qos to the old format with enum numbers - Fix downcast issue in the rclcpp::qos to Rosbag2QoS converter functions Signed-off-by: Michael Orlov --- .../include/rosbag2_storage/qos.hpp | 22 +++--- .../include/rosbag2_storage/yaml.hpp | 21 +++--- rosbag2_storage/src/rosbag2_storage/qos.cpp | 69 +++++++++---------- 3 files changed, 57 insertions(+), 55 deletions(-) diff --git a/rosbag2_storage/include/rosbag2_storage/qos.hpp b/rosbag2_storage/include/rosbag2_storage/qos.hpp index b07a0a083e..224f180901 100644 --- a/rosbag2_storage/include/rosbag2_storage/qos.hpp +++ b/rosbag2_storage/include/rosbag2_storage/qos.hpp @@ -71,7 +71,7 @@ class ROSBAG2_STORAGE_PUBLIC Rosbag2QoS : public rclcpp::QoS std::vector from_rclcpp_qos_vector( const std::vector & in); -std::string serialize_rclcpp_qos_vector(const std::vector & in); +std::string serialize_rclcpp_qos_vector(const std::vector & in, int version = 9); std::vector to_rclcpp_qos_vector(const std::string & serialized, int version); } // namespace rosbag2_storage @@ -101,12 +101,6 @@ T decode_for_version(const Node & node, int version) throw TypedBadConversion(node.Mark()); } -template -Node encode_for_version(const T & value, int version) -{ - return convert::encode(value, version); -} - template<> struct ROSBAG2_STORAGE_PUBLIC convert { @@ -142,10 +136,17 @@ struct ROSBAG2_STORAGE_PUBLIC convert 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); + static Node encode(const rosbag2_storage::Rosbag2QoS & qos, int version = 9); static bool decode(const Node & node, rosbag2_storage::Rosbag2QoS & qos, int version = 9); }; @@ -160,9 +161,8 @@ struct ROSBAG2_STORAGE_PUBLIC convert> 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); + static Node encode(const std::vector & rhs, int version = 9); + static bool decode(const Node & node, std::vector & rhs, int version = 9); }; template<> diff --git a/rosbag2_storage/include/rosbag2_storage/yaml.hpp b/rosbag2_storage/include/rosbag2_storage/yaml.hpp index d78229ff9a..6244a53bc7 100644 --- a/rosbag2_storage/include/rosbag2_storage/yaml.hpp +++ b/rosbag2_storage/include/rosbag2_storage/yaml.hpp @@ -69,14 +69,14 @@ struct convert> 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"] = rosbag2_storage::serialize_rclcpp_qos_vector( - topic.offered_qos_profiles); + node["offered_qos_profiles"] = + rosbag2_storage::serialize_rclcpp_qos_vector(topic.offered_qos_profiles, version); node["type_description_hash"] = topic.type_description_hash; return node; } @@ -102,11 +102,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; } @@ -122,11 +123,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; } @@ -217,7 +218,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/src/rosbag2_storage/qos.cpp b/rosbag2_storage/src/rosbag2_storage/qos.cpp index c492883237..2b5a0fd767 100644 --- a/rosbag2_storage/src/rosbag2_storage/qos.cpp +++ b/rosbag2_storage/src/rosbag2_storage/qos.cpp @@ -149,57 +149,45 @@ bool convert::decode(const Node & node, rmw_time_t & time) return true; } -Node convert::encode( - const rosbag2_storage::Rosbag2QoS & qos) +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, 9); + node["history"] = convert::encode(p.history, version); node["depth"] = p.depth; - node["reliability"] = convert::encode(p.reliability, 9); - node["durability"] = convert::encode(p.durability, 9); + 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, 9); + 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, rosbag2_storage::Rosbag2QoS & qos, int version) +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; - int temp; - if (convert::decode(node["history"], temp)) { - history = static_cast(temp); - } else { - history = node["history"].as(); - } - if (version <= 8) { + history = static_cast(node["history"].as()); reliability = static_cast(node["reliability"].as()); durability = static_cast(node["durability"].as()); liveliness = static_cast(node["liveliness"].as()); } else { + history = node["history"].as(); reliability = node["reliability"].as(); durability = node["durability"].as(); liveliness = node["liveliness"].as(); } - switch (history) { - case RMW_QOS_POLICY_HISTORY_KEEP_LAST: - qos.keep_last(node["depth"].as()); - break; - case RMW_QOS_POLICY_HISTORY_KEEP_ALL: - qos.keep_all(); - break; - default: - qos.history(history); + if (history == RMW_QOS_POLICY_HISTORY_KEEP_LAST) { + qos.keep_last(node["depth"].as()); + } else { + qos.history(history); } qos @@ -214,6 +202,18 @@ bool convert::decode( 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) { @@ -233,18 +233,16 @@ bool convert>::decode( rhs.clear(); for (const auto & value : node) { - auto temp = decode_for_version(value, version); - rhs.push_back(temp); + rhs.push_back(decode_for_version(value, version)); } return true; } -Node convert>::encode( - const std::vector & rhs) +Node convert>::encode(const std::vector & rhs, int version) { Node node{NodeType::Sequence}; for (const auto & value : rhs) { - node.push_back(static_cast(value)); + node.push_back(convert::encode(value, version)); } return node; } @@ -258,8 +256,10 @@ bool convert>::decode( rhs.clear(); for (const auto & value : node) { - auto temp = decode_for_version(value, version); - rhs.push_back(temp); + // 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; } @@ -414,7 +414,6 @@ Rosbag2QoS Rosbag2QoS::adapt_offer_to_recorded_offers( return Rosbag2QoS{}; } - std::vector from_rclcpp_qos_vector(const std::vector & in) { std::vector out; @@ -425,15 +424,15 @@ std::vector from_rclcpp_qos_vector(const std::vecto return out; } -std::string serialize_rclcpp_qos_vector(const std::vector & in) +std::string serialize_rclcpp_qos_vector(const std::vector & in, int version) { - auto node = YAML::convert>::encode(in); + 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 == "") {return {};} + if (serialized.empty()) {return {};} auto node = YAML::Load(serialized); return YAML::decode_for_version>(node, version); } From 377d1e6eef010804a3f90f5d4224b2a60f791392 Mon Sep 17 00:00:00 2001 From: Michael Orlov Date: Fri, 3 Nov 2023 15:30:44 -0700 Subject: [PATCH 44/51] Add qos serialization format auto-detection Signed-off-by: Michael Orlov --- rosbag2_py/src/rosbag2_py/_transport.cpp | 2 +- rosbag2_storage/src/rosbag2_storage/qos.cpp | 13 +++++++++++++ rosbag2_storage/test/rosbag2_storage/test_qos.cpp | 8 ++++---- 3 files changed, 18 insertions(+), 5 deletions(-) diff --git a/rosbag2_py/src/rosbag2_py/_transport.cpp b/rosbag2_py/src/rosbag2_py/_transport.cpp index d637f2dc14..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(); - record_options = YAML::decode_for_version(bag_node, 9); + 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/src/rosbag2_storage/qos.cpp b/rosbag2_storage/src/rosbag2_storage/qos.cpp index 2b5a0fd767..3a4221b3d0 100644 --- a/rosbag2_storage/src/rosbag2_storage/qos.cpp +++ b/rosbag2_storage/src/rosbag2_storage/qos.cpp @@ -172,6 +172,19 @@ bool convert::decode(const Node & node, rclcpp::QoS & qos, int vers 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) { history = static_cast(node["history"].as()); reliability = static_cast(node["reliability"].as()); diff --git a/rosbag2_storage/test/rosbag2_storage/test_qos.cpp b/rosbag2_storage/test/rosbag2_storage/test_qos.cpp index d8313bba75..39c4fa2cef 100644 --- a/rosbag2_storage/test/rosbag2_storage/test_qos.cpp +++ b/rosbag2_storage/test/rosbag2_storage/test_qos.cpp @@ -29,8 +29,7 @@ TEST(TestQoS, serialization) std::string serialized = YAML::Dump(offered_qos_profiles); YAML::Node loaded_node = YAML::Load(serialized); - auto deserialized_profiles = YAML::decode_for_version>( - loaded_node, 9); + auto deserialized_profiles = loaded_node.as>(); ASSERT_EQ(deserialized_profiles.size(), 1u); rosbag2_storage::Rosbag2QoS actual_qos = deserialized_profiles[0]; @@ -59,8 +58,9 @@ TEST(TestQoS, supports_version_4) " avoid_ros_namespace_conventions: false\n"; YAML::Node loaded_node = YAML::Load(serialized_profiles); - auto deserialized_profiles = YAML::decode_for_version>( - loaded_node, 8); + // 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(); From cae07aa36e191faff07673a44ec319f81b44005b Mon Sep 17 00:00:00 2001 From: Michael Orlov Date: Fri, 3 Nov 2023 16:00:07 -0700 Subject: [PATCH 45/51] Workaround to properly convert offered_qos_profiles in mcap storage - For old versions of the mcap files we were not storing metadata inside mcap files and metadata.version was assigned to the default 9 version. Signed-off-by: Michael Orlov --- rosbag2_storage_mcap/src/mcap_storage.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/rosbag2_storage_mcap/src/mcap_storage.cpp b/rosbag2_storage_mcap/src/mcap_storage.cpp index e330642dfe..29d2e712f6 100644 --- a/rosbag2_storage_mcap/src/mcap_storage.cpp +++ b/rosbag2_storage_mcap/src/mcap_storage.cpp @@ -400,7 +400,11 @@ 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"); } catch (const std::out_of_range & /* err */) { From 753e3e3d05018813c3ab057fb1e5cc1fbfd44188 Mon Sep 17 00:00:00 2001 From: roncapat Date: Sat, 4 Nov 2023 16:51:06 +0100 Subject: [PATCH 46/51] Fix clang format Signed-off-by: Patrick Roncagliolo --- rosbag2_storage_mcap/src/mcap_storage.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/rosbag2_storage_mcap/src/mcap_storage.cpp b/rosbag2_storage_mcap/src/mcap_storage.cpp index 29d2e712f6..20a6633abe 100644 --- a/rosbag2_storage_mcap/src/mcap_storage.cpp +++ b/rosbag2_storage_mcap/src/mcap_storage.cpp @@ -401,10 +401,10 @@ void MCAPStorage::read_metadata() 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 + 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"); } catch (const std::out_of_range & /* err */) { From dff7cdf5fb0a3d18c2e92bcebd4e4d5b00756992 Mon Sep 17 00:00:00 2001 From: Patrick Roncagliolo Date: Mon, 6 Nov 2023 10:00:09 +0100 Subject: [PATCH 47/51] Last fixes Signed-off-by: Patrick Roncagliolo --- rosbag2_cpp/test/rosbag2_cpp/test_info.cpp | 10 ++++++++-- rosbag2_storage/src/rosbag2_storage/qos.cpp | 2 -- 2 files changed, 8 insertions(+), 4 deletions(-) diff --git a/rosbag2_cpp/test/rosbag2_cpp/test_info.cpp b/rosbag2_cpp/test/rosbag2_cpp/test_info.cpp index 10039d35d8..2ba7701a50 100644 --- a/rosbag2_cpp/test/rosbag2_cpp/test_info.cpp +++ b/rosbag2_cpp/test/rosbag2_cpp/test_info.cpp @@ -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" diff --git a/rosbag2_storage/src/rosbag2_storage/qos.cpp b/rosbag2_storage/src/rosbag2_storage/qos.cpp index 3a4221b3d0..8c3e165d68 100644 --- a/rosbag2_storage/src/rosbag2_storage/qos.cpp +++ b/rosbag2_storage/src/rosbag2_storage/qos.cpp @@ -186,12 +186,10 @@ bool convert::decode(const Node & node, rclcpp::QoS & qos, int vers } if (version <= 8) { - history = static_cast(node["history"].as()); reliability = static_cast(node["reliability"].as()); durability = static_cast(node["durability"].as()); liveliness = static_cast(node["liveliness"].as()); } else { - history = node["history"].as(); reliability = node["reliability"].as(); durability = node["durability"].as(); liveliness = node["liveliness"].as(); From de95fbb1528733e49d06a3ce3c19a4b979d8665b Mon Sep 17 00:00:00 2001 From: Patrick Roncagliolo Date: Tue, 7 Nov 2023 11:19:22 +0100 Subject: [PATCH 48/51] Fix visibility Signed-off-by: Patrick Roncagliolo --- rosbag2_storage/include/rosbag2_storage/qos.hpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/rosbag2_storage/include/rosbag2_storage/qos.hpp b/rosbag2_storage/include/rosbag2_storage/qos.hpp index 224f180901..ce1ce471d5 100644 --- a/rosbag2_storage/include/rosbag2_storage/qos.hpp +++ b/rosbag2_storage/include/rosbag2_storage/qos.hpp @@ -69,10 +69,10 @@ class ROSBAG2_STORAGE_PUBLIC Rosbag2QoS : public rclcpp::QoS const std::vector & profiles); }; -std::vector from_rclcpp_qos_vector( +ROSBAG2_STORAGE_PUBLIC std::vector from_rclcpp_qos_vector( const std::vector & in); -std::string serialize_rclcpp_qos_vector(const std::vector & in, int version = 9); -std::vector to_rclcpp_qos_vector(const std::string & serialized, int version); +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 From 3bcc025e08eec514e0aafc74517e4fb4c6bd9947 Mon Sep 17 00:00:00 2001 From: Patrick Roncagliolo Date: Tue, 7 Nov 2023 11:32:14 +0100 Subject: [PATCH 49/51] Uncrust Signed-off-by: Patrick Roncagliolo --- rosbag2_storage/include/rosbag2_storage/qos.hpp | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/rosbag2_storage/include/rosbag2_storage/qos.hpp b/rosbag2_storage/include/rosbag2_storage/qos.hpp index ce1ce471d5..224ca7a042 100644 --- a/rosbag2_storage/include/rosbag2_storage/qos.hpp +++ b/rosbag2_storage/include/rosbag2_storage/qos.hpp @@ -71,8 +71,12 @@ class ROSBAG2_STORAGE_PUBLIC Rosbag2QoS : public rclcpp::QoS 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); +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 From fc19114183296da51e8876c57eca7260fa1eedc3 Mon Sep 17 00:00:00 2001 From: Patrick Roncagliolo Date: Wed, 8 Nov 2023 09:07:33 +0100 Subject: [PATCH 50/51] Fix DLL troubles with yaml-cpp Signed-off-by: Patrick Roncagliolo --- rosbag2_storage/include/rosbag2_storage/qos.hpp | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) diff --git a/rosbag2_storage/include/rosbag2_storage/qos.hpp b/rosbag2_storage/include/rosbag2_storage/qos.hpp index 224ca7a042..baba001ce0 100644 --- a/rosbag2_storage/include/rosbag2_storage/qos.hpp +++ b/rosbag2_storage/include/rosbag2_storage/qos.hpp @@ -21,8 +21,17 @@ #include "rclcpp/node_interfaces/node_graph_interface.hpp" #include "rclcpp/qos.hpp" -#include "yaml-cpp/yaml.h" +#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" From 39c453c41f4057d482ad431326a7a9e8fde92a6e Mon Sep 17 00:00:00 2001 From: Patrick Roncagliolo Date: Thu, 9 Nov 2023 09:48:37 +0100 Subject: [PATCH 51/51] Metadata V9 offered_qos_profiles as pure YAML Signed-off-by: Patrick Roncagliolo --- rosbag2_storage/include/rosbag2_storage/yaml.hpp | 14 +++++++++++--- 1 file changed, 11 insertions(+), 3 deletions(-) diff --git a/rosbag2_storage/include/rosbag2_storage/yaml.hpp b/rosbag2_storage/include/rosbag2_storage/yaml.hpp index 6244a53bc7..a71a830cc4 100644 --- a/rosbag2_storage/include/rosbag2_storage/yaml.hpp +++ b/rosbag2_storage/include/rosbag2_storage/yaml.hpp @@ -75,8 +75,13 @@ struct convert node["name"] = topic.name; node["type"] = topic.type; node["serialization_format"] = topic.serialization_format; - node["offered_qos_profiles"] = - rosbag2_storage::serialize_rclcpp_qos_vector(topic.offered_qos_profiles, version); + 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; } @@ -86,7 +91,10 @@ struct convert topic.name = node["name"].as(); topic.type = node["type"].as(); topic.serialization_format = node["serialization_format"].as(); - if (version >= 4) { + 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); }