From e6c64ef62ad759489d2e428577f1015183339444 Mon Sep 17 00:00:00 2001 From: Emerson Knapp Date: Mon, 23 Mar 2020 19:05:46 -0700 Subject: [PATCH 01/15] WIP serialize and deserialize QoS in metadata Signed-off-by: Emerson Knapp --- .../test_sequential_compression_reader.cpp | 2 +- rosbag2_cpp/test/rosbag2_cpp/test_info.cpp | 14 ++- .../rosbag2_cpp/test_multifile_reader.cpp | 2 +- .../rosbag2_cpp/test_sequential_writer.cpp | 8 +- .../test_storage_without_metadata_file.cpp | 3 +- .../rosbag2_storage/topic_metadata.hpp | 2 + .../src/rosbag2_storage/metadata_io.cpp | 4 +- .../test_metadata_serialization.cpp | 4 +- .../sqlite/sqlite_storage.cpp | 39 ++++--- .../sqlite/storage_test_fixture.hpp | 2 +- .../sqlite/test_sqlite_storage.cpp | 16 +-- rosbag2_tests/CMakeLists.txt | 8 ++ .../test/rosbag2_tests/test_rosbag2_qos.cpp | 46 ++++++++ .../src/rosbag2_transport/qos.hpp | 101 ++++++++++++++++++ .../test/rosbag2_transport/test_formatter.cpp | 4 +- .../test/rosbag2_transport/test_info.cpp | 4 +- .../test/rosbag2_transport/test_play.cpp | 4 +- .../rosbag2_transport/test_play_timing.cpp | 4 +- .../rosbag2_transport/test_rosbag2_node.cpp | 5 +- 19 files changed, 225 insertions(+), 47 deletions(-) create mode 100644 rosbag2_tests/test/rosbag2_tests/test_rosbag2_qos.cpp create mode 100644 rosbag2_transport/src/rosbag2_transport/qos.hpp 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 8f7b6fbf8..2362bf8f7 100644 --- a/rosbag2_compression/test/rosbag2_compression/test_sequential_compression_reader.cpp +++ b/rosbag2_compression/test/rosbag2_compression/test_sequential_compression_reader.cpp @@ -44,7 +44,7 @@ class SequentialCompressionReaderTest : public Test storage_serialization_format_{"rmw1_format"} { 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_cpp/test/rosbag2_cpp/test_info.cpp b/rosbag2_cpp/test/rosbag2_cpp/test_info.cpp index 36761db9f..8d6ff6850 100644 --- a/rosbag2_cpp/test/rosbag2_cpp/test_info.cpp +++ b/rosbag2_cpp/test/rosbag2_cpp/test_info.cpp @@ -49,11 +49,13 @@ TEST_F(TemporaryDirectoryFixture, read_metadata_supports_version_2) { " name: topic1\n" " type: type1\n" " serialization_format: rmw1\n" + " qos_profile: qosprofile1\n" " message_count: 100\n" " - topic_metadata:\n" " name: topic2\n" " type: type2\n" " serialization_format: rmw2\n" + " qos_profile: qosprofile2\n" " message_count: 200\n"; { @@ -82,7 +84,7 @@ TEST_F(TemporaryDirectoryFixture, 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", "qosprofile1"}, 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); @@ -91,7 +93,7 @@ TEST_F(TemporaryDirectoryFixture, 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", "qosprofile2"}, 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); @@ -117,11 +119,13 @@ TEST_F(TemporaryDirectoryFixture, read_metadata_makes_appropriate_call_to_metada " name: topic1\n" " type: type1\n" " serialization_format: rmw1\n" + " qos_profile: qos_profile1\n" " message_count: 100\n" " - topic_metadata:\n" " name: topic2\n" " type: type2\n" " serialization_format: rmw2\n" + " qos_profile: qos_profile2\n" " message_count: 200\n" " compression_format: \"zstd\"\n" " compression_mode: \"FILE\"\n"); @@ -148,7 +152,8 @@ TEST_F(TemporaryDirectoryFixture, read_metadata_makes_appropriate_call_to_metada EXPECT_THAT(read_metadata.message_count, Eq(50u)); 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}; + rosbag2_storage::TopicInformation expected_first_topic = + {{"topic1", "type1", "rmw1", "qosprofile1"}, 100}; EXPECT_THAT( actual_first_topic.topic_metadata.name, Eq(expected_first_topic.topic_metadata.name)); @@ -160,7 +165,8 @@ TEST_F(TemporaryDirectoryFixture, read_metadata_makes_appropriate_call_to_metada Eq(expected_first_topic.topic_metadata.serialization_format)); 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}; + rosbag2_storage::TopicInformation expected_second_topic = + {{"topic2", "type2", "rmw2", "qosprofile2"}, 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 a78c875c3..d0e903db3 100644 --- a/rosbag2_cpp/test/rosbag2_cpp/test_multifile_reader.cpp +++ b/rosbag2_cpp/test/rosbag2_cpp/test_multifile_reader.cpp @@ -43,7 +43,7 @@ class MultifileReaderTest : public Test relative_path_2_("some_relative_path_2") { 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}; auto message = std::make_shared(); diff --git a/rosbag2_cpp/test/rosbag2_cpp/test_sequential_writer.cpp b/rosbag2_cpp/test/rosbag2_cpp/test_sequential_writer.cpp index 2ecca1501..af2d6d143 100644 --- a/rosbag2_cpp/test/rosbag2_cpp/test_sequential_writer.cpp +++ b/rosbag2_cpp/test/rosbag2_cpp/test_sequential_writer.cpp @@ -93,7 +93,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); } @@ -110,7 +110,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); } @@ -179,7 +179,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); @@ -227,7 +227,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); 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 bae30812f..fd34ce4ce 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 @@ -55,7 +55,8 @@ TEST_F(StorageWithoutMetadataFileTest, open_uses_storage_id_from_storage_options auto topic_with_type = rosbag2_storage::TopicMetadata{ "topic", "test_msgs/BasicTypes", - kRmwFormat + kRmwFormat, + "" }; auto topic_information = rosbag2_storage::TopicInformation{ diff --git a/rosbag2_storage/include/rosbag2_storage/topic_metadata.hpp b/rosbag2_storage/include/rosbag2_storage/topic_metadata.hpp index 155b562a1..4b3ad9d09 100644 --- a/rosbag2_storage/include/rosbag2_storage/topic_metadata.hpp +++ b/rosbag2_storage/include/rosbag2_storage/topic_metadata.hpp @@ -25,6 +25,8 @@ struct TopicMetadata std::string name; std::string type; std::string serialization_format; + // std::vector serialized to YAML string + std::string offered_qos_profiles; bool operator==(const rosbag2_storage::TopicMetadata & rhs) const { diff --git a/rosbag2_storage/src/rosbag2_storage/metadata_io.cpp b/rosbag2_storage/src/rosbag2_storage/metadata_io.cpp index 95df940aa..d38d3c7f8 100644 --- a/rosbag2_storage/src/rosbag2_storage/metadata_io.cpp +++ b/rosbag2_storage/src/rosbag2_storage/metadata_io.cpp @@ -23,6 +23,7 @@ #include "rcutils/filesystem.h" #include "rosbag2_storage/topic_metadata.hpp" +#include "rosbag2_storage/logging.hpp" #ifdef _WIN32 // This is necessary because of a bug in yaml-cpp's cmake @@ -48,6 +49,7 @@ struct convert node["name"] = topic.name; node["type"] = topic.type; node["serialization_format"] = topic.serialization_format; + node["offered_qos_profiles"] = topic.offered_qos_profiles; return node; } @@ -56,7 +58,7 @@ struct convert topic.name = node["name"].as(); topic.type = node["type"].as(); topic.serialization_format = node["serialization_format"].as(); - + topic.offered_qos_profiles = node["offered_qos_profiles"].as(); 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 4c1f63f65..d6af71fab 100644 --- a/rosbag2_storage/test/rosbag2_storage/test_metadata_serialization.cpp +++ b/rosbag2_storage/test/rosbag2_storage/test_metadata_serialization.cpp @@ -53,8 +53,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"}, 100}); - metadata.topics_with_message_count.push_back({{"topic2", "type2", "rmw2"}, 200}); + metadata.topics_with_message_count.push_back({{"topic1", "type1", "rmw1", "qos_profile1"}, 100}); + metadata.topics_with_message_count.push_back({{"topic2", "type2", "rmw2", "qos_profile2"}, 200}); metadata_io_->write_metadata(temporary_dir_path_, metadata); auto read_metadata = metadata_io_->read_metadata(temporary_dir_path_); diff --git a/rosbag2_storage_default_plugins/src/rosbag2_storage_default_plugins/sqlite/sqlite_storage.cpp b/rosbag2_storage_default_plugins/src/rosbag2_storage_default_plugins/sqlite/sqlite_storage.cpp index 0a33fe23b..1e6f7fbd7 100644 --- a/rosbag2_storage_default_plugins/src/rosbag2_storage_default_plugins/sqlite/sqlite_storage.cpp +++ b/rosbag2_storage_default_plugins/src/rosbag2_storage_default_plugins/sqlite/sqlite_storage.cpp @@ -167,7 +167,8 @@ void SqliteStorage::initialize() "id INTEGER PRIMARY KEY," \ "name TEXT NOT NULL," \ "type TEXT NOT NULL," \ - "serialization_format TEXT NOT NULL);"; + "serialization_format TEXT NOT NULL," \ + "offered_qos_profiles TEXT NOT NULL);"; database_->prepare_statement(create_stmt)->execute_and_reset(); create_stmt = "CREATE TABLE messages(" \ "id INTEGER PRIMARY KEY," \ @@ -182,10 +183,12 @@ void SqliteStorage::initialize() void SqliteStorage::create_topic(const rosbag2_storage::TopicMetadata & topic) { if (topics_.find(topic.name) == std::end(topics_)) { + + ROSBAG2_STORAGE_DEFAULT_PLUGINS_LOG_ERROR_STREAM("sql creating topic " << topic.name << " with " << topic.offered_qos_profiles); auto insert_topic = database_->prepare_statement( - "INSERT INTO topics (name, type, serialization_format) VALUES (?, ?, ?)"); - insert_topic->bind(topic.name, topic.type, topic.serialization_format); + "INSERT INTO topics (name, type, serialization_format, offered_qos_profiles) VALUES (?, ?, ?, ?)"); + insert_topic->bind(topic.name, topic.type, topic.serialization_format, topic.offered_qos_profiles); insert_topic->execute_and_reset(); topics_.emplace(topic.name, static_cast(database_->get_last_insert_id())); } @@ -223,12 +226,12 @@ void SqliteStorage::prepare_for_reading() void SqliteStorage::fill_topics_and_types() { auto statement = database_->prepare_statement( - "SELECT name, type, serialization_format FROM topics ORDER BY id;"); - auto query_results = statement->execute_query(); + "SELECT name, type, serialization_format, offered_qos_profiles FROM topics ORDER BY id;"); + auto query_results = statement->execute_query(); 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), std::get<3>(result)}); } } @@ -257,26 +260,34 @@ rosbag2_storage::BagMetadata SqliteStorage::get_metadata() metadata.topics_with_message_count = {}; auto statement = database_->prepare_statement( - "SELECT name, type, serialization_format, COUNT(messages.id), MIN(messages.timestamp), " - "MAX(messages.timestamp) " + "SELECT name, type, serialization_format, offered_qos_profiles, " + "COUNT(messages.id), MIN(messages.timestamp), MAX(messages.timestamp) " "FROM messages JOIN topics on topics.id = messages.topic_id " "GROUP BY topics.name;"); auto query_results = statement->execute_query< - std::string, std::string, std::string, int, rcutils_time_point_value_t, + std::string, std::string, std::string, std::string, int, rcutils_time_point_value_t, rcutils_time_point_value_t>(); rcutils_time_point_value_t min_time = INT64_MAX; rcutils_time_point_value_t max_time = 0; for (auto result : query_results) { + std::string name = std::get<0>(result); + std::string type = std::get<1>(result); + std::string serialization_format = std::get<2>(result); + std::string offered_qos_profiles = std::get<3>(result); + + ROSBAG2_STORAGE_DEFAULT_PLUGINS_LOG_ERROR_STREAM( + "READING METADATA FOR TOPIC " << name << " qos " << offered_qos_profiles); + metadata.topics_with_message_count.push_back( { - {std::get<0>(result), std::get<1>(result), std::get<2>(result)}, - static_cast(std::get<3>(result)) + {name, type, serialization_format, offered_qos_profiles}, + static_cast(std::get<4>(result)) }); - metadata.message_count += std::get<3>(result); - min_time = std::get<4>(result) < min_time ? std::get<4>(result) : min_time; - max_time = std::get<5>(result) > max_time ? std::get<5>(result) : max_time; + metadata.message_count += std::get<4>(result); + min_time = std::get<4>(result) < min_time ? std::get<5>(result) : min_time; + max_time = std::get<5>(result) > max_time ? std::get<6>(result) : max_time; } if (metadata.message_count == 0) { diff --git a/rosbag2_storage_default_plugins/test/rosbag2_storage_default_plugins/sqlite/storage_test_fixture.hpp b/rosbag2_storage_default_plugins/test/rosbag2_storage_default_plugins/sqlite/storage_test_fixture.hpp index 3c2d263cf..5b04ba50d 100644 --- a/rosbag2_storage_default_plugins/test/rosbag2_storage_default_plugins/sqlite/storage_test_fixture.hpp +++ b/rosbag2_storage_default_plugins/test/rosbag2_storage_default_plugins/sqlite/storage_test_fixture.hpp @@ -105,7 +105,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); - writable_storage->create_topic({topic_name, type_name, rmw_format}); + writable_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_default_plugins/test/rosbag2_storage_default_plugins/sqlite/test_sqlite_storage.cpp b/rosbag2_storage_default_plugins/test/rosbag2_storage_default_plugins/sqlite/test_sqlite_storage.cpp index e989f5d86..b356ed8a3 100644 --- a/rosbag2_storage_default_plugins/test/rosbag2_storage_default_plugins/sqlite/test_sqlite_storage.cpp +++ b/rosbag2_storage_default_plugins/test/rosbag2_storage_default_plugins/sqlite/test_sqlite_storage.cpp @@ -119,8 +119,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); - writable_storage->create_topic({"topic1", "type1", "rmw1"}); - writable_storage->create_topic({"topic2", "type2", "rmw2"}); + writable_storage->create_topic({"topic1", "type1", "rmw1", {}}); + writable_storage->create_topic({"topic2", "type2", "rmw2", {}}); const auto read_only_filename = writable_storage->get_relative_file_path(); @@ -134,8 +134,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"}, - rosbag2_storage::TopicMetadata{"topic2", "type2", "rmw2"} + rosbag2_storage::TopicMetadata{"topic1", "type1", "rmw1", {}}, + rosbag2_storage::TopicMetadata{"topic2", "type2", "rmw2", {}} })); } @@ -164,9 +164,9 @@ TEST_F(StorageTestFixture, get_metadata_returns_correct_struct) { 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( @@ -204,8 +204,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); - writable_storage->create_topic({"topic1", "type1", "rmw1"}); - writable_storage->remove_topic({"topic1", "type1", "rmw1"}); + writable_storage->create_topic({"topic1", "type1", "rmw1", {}}); + writable_storage->remove_topic({"topic1", "type1", "rmw1", {}}); const auto read_only_filename = writable_storage->get_relative_file_path(); diff --git a/rosbag2_tests/CMakeLists.txt b/rosbag2_tests/CMakeLists.txt index c27a5eac8..cc7335451 100644 --- a/rosbag2_tests/CMakeLists.txt +++ b/rosbag2_tests/CMakeLists.txt @@ -67,6 +67,14 @@ if(BUILD_TESTING) # test_msgs) # endif() + # ament_add_gmock(test_rosbag2_qos + # test/rosbag2_tests/test_rosbag2_qos.cpp + # WORKING_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}) + # if(TARGET test_rosbag2_qos) + # ament_target_dependencies(test_rosbag2_qos + # rosbag2_transport) + # endif() + ament_add_gmock(test_rosbag2_info_end_to_end test/rosbag2_tests/test_rosbag2_info_end_to_end.cpp WORKING_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}) diff --git a/rosbag2_tests/test/rosbag2_tests/test_rosbag2_qos.cpp b/rosbag2_tests/test/rosbag2_tests/test_rosbag2_qos.cpp new file mode 100644 index 000000000..f683e4427 --- /dev/null +++ b/rosbag2_tests/test/rosbag2_tests/test_rosbag2_qos.cpp @@ -0,0 +1,46 @@ +#include + +#include "rosbag2_transport/play_options.hpp" +#include "rosbag2_transport/storage_options.hpp" + +namespace +{} + +TEST() { + rosbag2_transport::PlayOptions play_options{}; + play_options.node_prefix = ""; + play_options.read_ahead_queue_size = read_ahead_queue_size; + + rosbag2_transport::StorageOptions storage_options{}; + storage_options.uri = ""; + storage_options.storage_id = ""; + + size_t read_ahead_queue_size; + + rosbag2_storage::MetadataIo metadata_io{}; + rosbag2_storage::BagMetadata metadata{}; + // Specify defaults + auto info = std::make_shared(); + std::shared_ptr reader; + auto writer = std::make_shared( + std::make_unique()); + // Change reader based on metadata options + if (metadata_io.metadata_file_exists(storage_options.uri)) { + metadata = metadata_io.read_metadata(storage_options.uri); + if (metadata.compression_format == "zstd") { + reader = std::make_shared( + std::make_unique()); + } else { + reader = std::make_shared( + std::make_unique()); + } + } else { + reader = std::make_shared( + std::make_unique()); + } + + rosbag2_transport::Rosbag2Transport transport(reader, writer, info); + transport.init(); + transport.play(storage_options, play_options); + transport.shutdown(); +} diff --git a/rosbag2_transport/src/rosbag2_transport/qos.hpp b/rosbag2_transport/src/rosbag2_transport/qos.hpp new file mode 100644 index 000000000..5c9d3834d --- /dev/null +++ b/rosbag2_transport/src/rosbag2_transport/qos.hpp @@ -0,0 +1,101 @@ +// Copyright 2020 Amazon.com, Inc. or its affiliates. All Rights Reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef ROSBAG2_TRANSPORT__QOS_HPP_ +#define ROSBAG2_TRANSPORT__QOS_HPP_ + +#include "rclcpp/qos.hpp" + +#ifdef _WIN32 +// This is necessary because of a bug in yaml-cpp's cmake +#define YAML_CPP_DLL +// 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 + +namespace rosbag2_transport +{ +class Rosbag2QoS : public rclcpp::QoS +{ +public: + explicit Rosbag2QoS() : rclcpp::QoS(10) {} + explicit Rosbag2QoS(const rclcpp::QoS & other) : rclcpp::QoS(other) {} +}; +} // namespace rosbag2_transport + + +namespace YAML +{ +template<> +struct convert +{ + static Node encode(const rmw_time_t & time) + { + Node node; + node["sec"] = time.sec; + node["nsec"] = time.nsec; + return node; + } + static bool decode(const Node & node, rmw_time_t & time) + { + time.sec = node["sec"].as(); + time.nsec = node["nsec"].as(); + return true; + } +}; + +template<> +struct convert +{ + static Node encode(const rosbag2_transport::Rosbag2QoS & qos) + { + const auto & p = qos.get_rmw_qos_profile(); + Node node; + node["history"] = (int)p.history; + node["depth"] = p.depth; + node["reliability"] = (int)p.reliability; + node["durability"] = (int)p.durability; + node["deadline"] = p.deadline; + node["lifespan"] = p.lifespan; + node["liveliness"] = (int)p.liveliness; + node["liveliness_lease_duration"] = p.liveliness_lease_duration; + node["avoid_ros_namespace_conventions"] = p.avoid_ros_namespace_conventions; + return node; + } + static bool decode(const Node & node, rosbag2_transport::Rosbag2QoS & qos) + { + qos + .keep_last(node["depth"].as()) + .history((rmw_qos_history_policy_t)node["history"].as()) + .reliability((rmw_qos_reliability_policy_t)node["reliability"].as()) + .durability((rmw_qos_durability_policy_t)node["durability"].as()) + .deadline(node["deadline"].as()) + .lifespan(node["lifespan"].as()) + .liveliness((rmw_qos_liveliness_policy_t)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 + + +#endif diff --git a/rosbag2_transport/test/rosbag2_transport/test_formatter.cpp b/rosbag2_transport/test/rosbag2_transport/test_formatter.cpp index 84f3a578c..47fdbdd68 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_formatter.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_formatter.cpp @@ -74,8 +74,8 @@ TEST_F(FormatterTestFixture, format_files_prints_newline_if_there_are_no_paths) TEST_F(FormatterTestFixture, format_topics_with_type_correctly_layouts_more_topics) { std::vector topics; - topics.push_back({{"topic1", "type1", "rmw1"}, 100}); - topics.push_back({{"topic2", "type2", "rmw2"}, 200}); + topics.push_back({{"topic1", "type1", "rmw1", "qos_profile1"}, 100}); + topics.push_back({{"topic2", "type2", "rmw2", "qos_profile2"}, 200}); std::stringstream formatted_output; formatter_->format_topics_with_type(topics, formatted_output, indentation_spaces_); diff --git a/rosbag2_transport/test/rosbag2_transport/test_info.cpp b/rosbag2_transport/test/rosbag2_transport/test_info.cpp index fbb858d00..0f8f2ef17 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_info.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_info.cpp @@ -36,8 +36,8 @@ TEST_F(Rosbag2TransportTestFixture, info_pretty_prints_information_from_bagfile) std::chrono::nanoseconds(1538051985348887471)); // corresponds to Sept 27 14:39:45.348 bagfile.duration = std::chrono::nanoseconds(50000000); bagfile.message_count = 50; - bagfile.topics_with_message_count.push_back({{"topic1", "type1", "rmw1"}, 100}); - bagfile.topics_with_message_count.push_back({{"topic2", "type2", "rmw2"}, 200}); + bagfile.topics_with_message_count.push_back({{"topic1", "type1", "rmw1", "qos_profile1"}, 100}); + bagfile.topics_with_message_count.push_back({{"topic2", "type2", "rmw2", "qos_profile2"}, 200}); EXPECT_CALL(*info_, read_metadata(_, _)).WillOnce(Return(bagfile)); // the expected output uses a regex to handle different time zones. diff --git a/rosbag2_transport/test/rosbag2_transport/test_play.cpp b/rosbag2_transport/test/rosbag2_transport/test_play.cpp index 1fdb2a3a6..fedadd7a2 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_play.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_play.cpp @@ -66,8 +66,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 = diff --git a/rosbag2_transport/test/rosbag2_transport/test_play_timing.cpp b/rosbag2_transport/test/rosbag2_transport/test_play_timing.cpp index 7d7959df1..695830eeb 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_play_timing.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_play_timing.cpp @@ -43,7 +43,7 @@ TEST_F(Rosbag2TransportTestFixture, playing_respects_relative_timing_of_stored_m auto message_time_difference = std::chrono::seconds(1); 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)}; @@ -79,7 +79,7 @@ TEST_F(Rosbag2TransportTestFixture, playing_respects_rate) auto message_time_difference = std::chrono::seconds(1); 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_rosbag2_node.cpp b/rosbag2_transport/test/rosbag2_transport/test_rosbag2_node.cpp index 1539d5c33..a685bb18d 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_rosbag2_node.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_rosbag2_node.cpp @@ -64,7 +64,7 @@ class RosBag2NodeFixture : public Test std::vector messages; size_t counter = 0; auto subscription = node_->create_generic_subscription( - topic_name, type, + topic_name, type, rclcpp::QoS(rclcpp::KeepAll()), [this, &counter, &messages](std::shared_ptr message) { auto string_message = memory_management_.deserialize_message(message); @@ -105,7 +105,8 @@ TEST_F(RosBag2NodeFixture, publisher_and_subscriber_work) std::string topic_name = "string_topic"; std::string type = "test_msgs/Strings"; - auto publisher = node_->create_generic_publisher(topic_name, type); + auto publisher = node_->create_generic_publisher( + topic_name, type, rclcpp::QoS(rclcpp::KeepAll())); auto subscriber_future_ = std::async( std::launch::async, [this, topic_name, type] { From 6ee2b30f94919687531babf76645f895d254f00a Mon Sep 17 00:00:00 2001 From: Emerson Knapp Date: Mon, 23 Mar 2020 22:46:19 -0700 Subject: [PATCH 02/15] Get the qos profiles Signed-off-by: Emerson Knapp --- .../src/rosbag2_transport/recorder.cpp | 37 ++++++++++++++++++- .../src/rosbag2_transport/recorder.hpp | 6 ++- 2 files changed, 40 insertions(+), 3 deletions(-) diff --git a/rosbag2_transport/src/rosbag2_transport/recorder.cpp b/rosbag2_transport/src/rosbag2_transport/recorder.cpp index 2551bed70..dc16d7f68 100644 --- a/rosbag2_transport/src/rosbag2_transport/recorder.cpp +++ b/rosbag2_transport/src/rosbag2_transport/recorder.cpp @@ -29,6 +29,7 @@ #include "generic_subscription.hpp" #include "rosbag2_node.hpp" +#include "qos.hpp" namespace rosbag2_transport { @@ -109,12 +110,44 @@ void Recorder::subscribe_topics( const std::unordered_map & topics_and_types) { for (const auto & topic_with_type : topics_and_types) { - subscribe_topic({topic_with_type.first, topic_with_type.second, serialization_format_}); + subscribe_topic(topic_with_type.first, topic_with_type.second, serialization_format_); } } -void Recorder::subscribe_topic(const rosbag2_storage::TopicMetadata & topic) +void Recorder::subscribe_topic(std::string name, std::string type, std::string serialization_format) { + rosbag2_storage::TopicMetadata topic {name, type, serialization_format, {}}; + rclcpp::QoS last_qos(10); + bool first = true; + bool all_qos_same = true; + rclcpp::QoS subscription_qos(10); + + auto publishers_info = node_->get_publishers_info_by_topic(topic.name); + ROSBAG2_TRANSPORT_LOG_ERROR_STREAM("Endpoints for topic " << topic.name); + YAML::Node offered_qos_profiles; + for (auto info : publishers_info) { + offered_qos_profiles.push_back(Rosbag2QoS(info.qos_profile())); + if (!first) { + all_qos_same &= last_qos == info.qos_profile(); + } + first = false; + last_qos = info.qos_profile(); + ROSBAG2_TRANSPORT_LOG_INFO_STREAM("---" << std::endl << last_qos); + } + + topic.offered_qos_profiles = YAML::Dump(offered_qos_profiles); + if (all_qos_same) { + subscription_qos = last_qos; + ROSBAG2_TRANSPORT_LOG_INFO_STREAM( + "OK! All publishers for topic " << topic.name << + " offering the same QoS profile - using it to subscribe."); + } else { + ROSBAG2_TRANSPORT_LOG_WARN_STREAM( + "Topic " << topic.name << " has publishers offering different QoS settings. " + "Can't guess what QoS to request, falling back to default QoS profile." + ); + } + // Need to create topic in writer before we are trying to create subscription. Since in // callback for subscription we are calling writer_->write(bag_message); and it could happened // that callback called before we reached out the line: writer_->create_topic(topic) diff --git a/rosbag2_transport/src/rosbag2_transport/recorder.hpp b/rosbag2_transport/src/rosbag2_transport/recorder.hpp index 3af47ff79..5e8086433 100644 --- a/rosbag2_transport/src/rosbag2_transport/recorder.hpp +++ b/rosbag2_transport/src/rosbag2_transport/recorder.hpp @@ -23,6 +23,7 @@ #include #include +#include "rclcpp/qos.hpp" #include "rosbag2_cpp/writer.hpp" #include "rosbag2_storage/topic_metadata.hpp" @@ -64,7 +65,10 @@ class Recorder void subscribe_topics( const std::unordered_map & topics_and_types); - void subscribe_topic(const rosbag2_storage::TopicMetadata & topic); + void subscribe_topic( + std::string name, + std::string type, + std::string serialization_format); std::shared_ptr create_subscription( const std::string & topic_name, const std::string & topic_type); From 9d86df5f5c88762b433e65bb6855df5b4943c0f5 Mon Sep 17 00:00:00 2001 From: Emerson Knapp Date: Tue, 24 Mar 2020 10:50:47 -0700 Subject: [PATCH 03/15] Fixing up tests Signed-off-by: Emerson Knapp --- .../test_sequential_compression_reader.cpp | 2 +- .../src/rosbag2_storage/metadata_io.cpp | 1 - .../sqlite/sqlite_storage.cpp | 12 +----------- .../sqlite/storage_test_fixture.hpp | 2 +- .../sqlite/test_sqlite_storage.cpp | 16 ++++++++-------- .../src/rosbag2_transport/recorder.hpp | 6 +----- .../test/rosbag2_transport/test_rosbag2_node.cpp | 5 ++--- 7 files changed, 14 insertions(+), 30 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 2362bf8f7..8f7b6fbf8 100644 --- a/rosbag2_compression/test/rosbag2_compression/test_sequential_compression_reader.cpp +++ b/rosbag2_compression/test/rosbag2_compression/test_sequential_compression_reader.cpp @@ -44,7 +44,7 @@ class SequentialCompressionReaderTest : public Test storage_serialization_format_{"rmw1_format"} { 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_storage/src/rosbag2_storage/metadata_io.cpp b/rosbag2_storage/src/rosbag2_storage/metadata_io.cpp index d38d3c7f8..932e46e71 100644 --- a/rosbag2_storage/src/rosbag2_storage/metadata_io.cpp +++ b/rosbag2_storage/src/rosbag2_storage/metadata_io.cpp @@ -23,7 +23,6 @@ #include "rcutils/filesystem.h" #include "rosbag2_storage/topic_metadata.hpp" -#include "rosbag2_storage/logging.hpp" #ifdef _WIN32 // This is necessary because of a bug in yaml-cpp's cmake diff --git a/rosbag2_storage_default_plugins/src/rosbag2_storage_default_plugins/sqlite/sqlite_storage.cpp b/rosbag2_storage_default_plugins/src/rosbag2_storage_default_plugins/sqlite/sqlite_storage.cpp index 1e6f7fbd7..9104e86f3 100644 --- a/rosbag2_storage_default_plugins/src/rosbag2_storage_default_plugins/sqlite/sqlite_storage.cpp +++ b/rosbag2_storage_default_plugins/src/rosbag2_storage_default_plugins/sqlite/sqlite_storage.cpp @@ -183,8 +183,6 @@ void SqliteStorage::initialize() void SqliteStorage::create_topic(const rosbag2_storage::TopicMetadata & topic) { if (topics_.find(topic.name) == std::end(topics_)) { - - ROSBAG2_STORAGE_DEFAULT_PLUGINS_LOG_ERROR_STREAM("sql creating topic " << topic.name << " with " << topic.offered_qos_profiles); auto insert_topic = database_->prepare_statement( "INSERT INTO topics (name, type, serialization_format, offered_qos_profiles) VALUES (?, ?, ?, ?)"); @@ -271,17 +269,9 @@ rosbag2_storage::BagMetadata SqliteStorage::get_metadata() rcutils_time_point_value_t min_time = INT64_MAX; rcutils_time_point_value_t max_time = 0; for (auto result : query_results) { - std::string name = std::get<0>(result); - std::string type = std::get<1>(result); - std::string serialization_format = std::get<2>(result); - std::string offered_qos_profiles = std::get<3>(result); - - ROSBAG2_STORAGE_DEFAULT_PLUGINS_LOG_ERROR_STREAM( - "READING METADATA FOR TOPIC " << name << " qos " << offered_qos_profiles); - metadata.topics_with_message_count.push_back( { - {name, type, serialization_format, offered_qos_profiles}, + {std::get<0>(result), std::get<1>(result), std::get<2>(result), std::get<3>(result)}, static_cast(std::get<4>(result)) }); diff --git a/rosbag2_storage_default_plugins/test/rosbag2_storage_default_plugins/sqlite/storage_test_fixture.hpp b/rosbag2_storage_default_plugins/test/rosbag2_storage_default_plugins/sqlite/storage_test_fixture.hpp index 5b04ba50d..200f650d8 100644 --- a/rosbag2_storage_default_plugins/test/rosbag2_storage_default_plugins/sqlite/storage_test_fixture.hpp +++ b/rosbag2_storage_default_plugins/test/rosbag2_storage_default_plugins/sqlite/storage_test_fixture.hpp @@ -105,7 +105,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); - writable_storage->create_topic({topic_name, type_name, rmw_format, {}}); + writable_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_default_plugins/test/rosbag2_storage_default_plugins/sqlite/test_sqlite_storage.cpp b/rosbag2_storage_default_plugins/test/rosbag2_storage_default_plugins/sqlite/test_sqlite_storage.cpp index b356ed8a3..08b1dd531 100644 --- a/rosbag2_storage_default_plugins/test/rosbag2_storage_default_plugins/sqlite/test_sqlite_storage.cpp +++ b/rosbag2_storage_default_plugins/test/rosbag2_storage_default_plugins/sqlite/test_sqlite_storage.cpp @@ -119,8 +119,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); - writable_storage->create_topic({"topic1", "type1", "rmw1", {}}); - writable_storage->create_topic({"topic2", "type2", "rmw2", {}}); + writable_storage->create_topic({"topic1", "type1", "rmw1", ""}); + writable_storage->create_topic({"topic2", "type2", "rmw2", ""}); const auto read_only_filename = writable_storage->get_relative_file_path(); @@ -134,8 +134,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", {}}, - rosbag2_storage::TopicMetadata{"topic2", "type2", "rmw2", {}} + rosbag2_storage::TopicMetadata{"topic1", "type1", "rmw1", ""}, + rosbag2_storage::TopicMetadata{"topic2", "type2", "rmw2", ""} })); } @@ -164,9 +164,9 @@ TEST_F(StorageTestFixture, get_metadata_returns_correct_struct) { 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( @@ -204,8 +204,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); - writable_storage->create_topic({"topic1", "type1", "rmw1", {}}); - writable_storage->remove_topic({"topic1", "type1", "rmw1", {}}); + writable_storage->create_topic({"topic1", "type1", "rmw1", ""}); + writable_storage->remove_topic({"topic1", "type1", "rmw1", ""}); const auto read_only_filename = writable_storage->get_relative_file_path(); diff --git a/rosbag2_transport/src/rosbag2_transport/recorder.hpp b/rosbag2_transport/src/rosbag2_transport/recorder.hpp index 5e8086433..3af47ff79 100644 --- a/rosbag2_transport/src/rosbag2_transport/recorder.hpp +++ b/rosbag2_transport/src/rosbag2_transport/recorder.hpp @@ -23,7 +23,6 @@ #include #include -#include "rclcpp/qos.hpp" #include "rosbag2_cpp/writer.hpp" #include "rosbag2_storage/topic_metadata.hpp" @@ -65,10 +64,7 @@ class Recorder void subscribe_topics( const std::unordered_map & topics_and_types); - void subscribe_topic( - std::string name, - std::string type, - std::string serialization_format); + void subscribe_topic(const rosbag2_storage::TopicMetadata & topic); std::shared_ptr create_subscription( const std::string & topic_name, const std::string & topic_type); diff --git a/rosbag2_transport/test/rosbag2_transport/test_rosbag2_node.cpp b/rosbag2_transport/test/rosbag2_transport/test_rosbag2_node.cpp index a685bb18d..1539d5c33 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_rosbag2_node.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_rosbag2_node.cpp @@ -64,7 +64,7 @@ class RosBag2NodeFixture : public Test std::vector messages; size_t counter = 0; auto subscription = node_->create_generic_subscription( - topic_name, type, rclcpp::QoS(rclcpp::KeepAll()), + topic_name, type, [this, &counter, &messages](std::shared_ptr message) { auto string_message = memory_management_.deserialize_message(message); @@ -105,8 +105,7 @@ TEST_F(RosBag2NodeFixture, publisher_and_subscriber_work) std::string topic_name = "string_topic"; std::string type = "test_msgs/Strings"; - auto publisher = node_->create_generic_publisher( - topic_name, type, rclcpp::QoS(rclcpp::KeepAll())); + auto publisher = node_->create_generic_publisher(topic_name, type); auto subscriber_future_ = std::async( std::launch::async, [this, topic_name, type] { From 1dfc541c714afd6187117333b48f7425c710ad27 Mon Sep 17 00:00:00 2001 From: Emerson Knapp Date: Tue, 24 Mar 2020 10:56:50 -0700 Subject: [PATCH 04/15] Build passing again Signed-off-by: Emerson Knapp --- .../test_sequential_compression_reader.cpp | 2 +- .../src/rosbag2_transport/recorder.cpp | 14 +++++++++----- 2 files changed, 10 insertions(+), 6 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 8f7b6fbf8..2362bf8f7 100644 --- a/rosbag2_compression/test/rosbag2_compression/test_sequential_compression_reader.cpp +++ b/rosbag2_compression/test/rosbag2_compression/test_sequential_compression_reader.cpp @@ -44,7 +44,7 @@ class SequentialCompressionReaderTest : public Test storage_serialization_format_{"rmw1_format"} { 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_transport/src/rosbag2_transport/recorder.cpp b/rosbag2_transport/src/rosbag2_transport/recorder.cpp index dc16d7f68..c815b84de 100644 --- a/rosbag2_transport/src/rosbag2_transport/recorder.cpp +++ b/rosbag2_transport/src/rosbag2_transport/recorder.cpp @@ -110,20 +110,24 @@ void Recorder::subscribe_topics( const std::unordered_map & topics_and_types) { for (const auto & topic_with_type : topics_and_types) { - subscribe_topic(topic_with_type.first, topic_with_type.second, serialization_format_); + subscribe_topic({topic_with_type.first, topic_with_type.second, serialization_format_, ""}); } } -void Recorder::subscribe_topic(std::string name, std::string type, std::string serialization_format) +void Recorder::subscribe_topic(const rosbag2_storage::TopicMetadata & topic_without_qos) { - rosbag2_storage::TopicMetadata topic {name, type, serialization_format, {}}; + rosbag2_storage::TopicMetadata topic { + topic_without_qos.name, + topic_without_qos.type, + topic_without_qos.serialization_format, + ""}; rclcpp::QoS last_qos(10); bool first = true; bool all_qos_same = true; rclcpp::QoS subscription_qos(10); auto publishers_info = node_->get_publishers_info_by_topic(topic.name); - ROSBAG2_TRANSPORT_LOG_ERROR_STREAM("Endpoints for topic " << topic.name); + ROSBAG2_TRANSPORT_LOG_ERROR_STREAM("Querying endpoints for topic " << topic.name); YAML::Node offered_qos_profiles; for (auto info : publishers_info) { offered_qos_profiles.push_back(Rosbag2QoS(info.qos_profile())); @@ -132,7 +136,7 @@ void Recorder::subscribe_topic(std::string name, std::string type, std::string s } first = false; last_qos = info.qos_profile(); - ROSBAG2_TRANSPORT_LOG_INFO_STREAM("---" << std::endl << last_qos); + // ROSBAG2_TRANSPORT_LOG_INFO_STREAM("---" << std::endl << last_qos); } topic.offered_qos_profiles = YAML::Dump(offered_qos_profiles); From 90b93dbe96b74240430dbbac91e26bb4bbcc3741 Mon Sep 17 00:00:00 2001 From: Emerson Knapp Date: Wed, 25 Mar 2020 15:08:14 -0700 Subject: [PATCH 05/15] Minimize changes Signed-off-by: Emerson Knapp --- rosbag2_cpp/test/rosbag2_cpp/test_info.cpp | 12 ++--- .../src/rosbag2_storage/metadata_io.cpp | 1 - .../test_metadata_serialization.cpp | 4 +- .../sqlite/sqlite_storage.cpp | 22 ++++----- rosbag2_tests/CMakeLists.txt | 8 ---- .../test/rosbag2_tests/test_rosbag2_qos.cpp | 46 ------------------- .../test/rosbag2_transport/test_formatter.cpp | 4 +- .../test/rosbag2_transport/test_info.cpp | 4 +- 8 files changed, 21 insertions(+), 80 deletions(-) delete mode 100644 rosbag2_tests/test/rosbag2_tests/test_rosbag2_qos.cpp diff --git a/rosbag2_cpp/test/rosbag2_cpp/test_info.cpp b/rosbag2_cpp/test/rosbag2_cpp/test_info.cpp index 8d6ff6850..0f4686803 100644 --- a/rosbag2_cpp/test/rosbag2_cpp/test_info.cpp +++ b/rosbag2_cpp/test/rosbag2_cpp/test_info.cpp @@ -49,13 +49,11 @@ TEST_F(TemporaryDirectoryFixture, read_metadata_supports_version_2) { " name: topic1\n" " type: type1\n" " serialization_format: rmw1\n" - " qos_profile: qosprofile1\n" " message_count: 100\n" " - topic_metadata:\n" " name: topic2\n" " type: type2\n" " serialization_format: rmw2\n" - " qos_profile: qosprofile2\n" " message_count: 200\n"; { @@ -84,7 +82,7 @@ TEST_F(TemporaryDirectoryFixture, 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", "qosprofile1"}, 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); @@ -93,7 +91,7 @@ TEST_F(TemporaryDirectoryFixture, 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", "qosprofile2"}, 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); @@ -119,13 +117,11 @@ TEST_F(TemporaryDirectoryFixture, read_metadata_makes_appropriate_call_to_metada " name: topic1\n" " type: type1\n" " serialization_format: rmw1\n" - " qos_profile: qos_profile1\n" " message_count: 100\n" " - topic_metadata:\n" " name: topic2\n" " type: type2\n" " serialization_format: rmw2\n" - " qos_profile: qos_profile2\n" " message_count: 200\n" " compression_format: \"zstd\"\n" " compression_mode: \"FILE\"\n"); @@ -153,7 +149,7 @@ TEST_F(TemporaryDirectoryFixture, read_metadata_makes_appropriate_call_to_metada 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", "qosprofile1"}, 100}; + {{"topic1", "type1", "rmw1", ""}, 100}; EXPECT_THAT( actual_first_topic.topic_metadata.name, Eq(expected_first_topic.topic_metadata.name)); @@ -166,7 +162,7 @@ TEST_F(TemporaryDirectoryFixture, read_metadata_makes_appropriate_call_to_metada 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", "qosprofile2"}, 200}; + {{"topic2", "type2", "rmw2", ""}, 200}; EXPECT_THAT( actual_second_topic.topic_metadata.name, Eq(expected_second_topic.topic_metadata.name)); diff --git a/rosbag2_storage/src/rosbag2_storage/metadata_io.cpp b/rosbag2_storage/src/rosbag2_storage/metadata_io.cpp index 932e46e71..4da663c98 100644 --- a/rosbag2_storage/src/rosbag2_storage/metadata_io.cpp +++ b/rosbag2_storage/src/rosbag2_storage/metadata_io.cpp @@ -57,7 +57,6 @@ struct convert topic.name = node["name"].as(); topic.type = node["type"].as(); topic.serialization_format = node["serialization_format"].as(); - topic.offered_qos_profiles = node["offered_qos_profiles"].as(); 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 d6af71fab..63c268637 100644 --- a/rosbag2_storage/test/rosbag2_storage/test_metadata_serialization.cpp +++ b/rosbag2_storage/test/rosbag2_storage/test_metadata_serialization.cpp @@ -53,8 +53,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", "qos_profile1"}, 100}); - metadata.topics_with_message_count.push_back({{"topic2", "type2", "rmw2", "qos_profile2"}, 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_); diff --git a/rosbag2_storage_default_plugins/src/rosbag2_storage_default_plugins/sqlite/sqlite_storage.cpp b/rosbag2_storage_default_plugins/src/rosbag2_storage_default_plugins/sqlite/sqlite_storage.cpp index 9104e86f3..dd90a045d 100644 --- a/rosbag2_storage_default_plugins/src/rosbag2_storage_default_plugins/sqlite/sqlite_storage.cpp +++ b/rosbag2_storage_default_plugins/src/rosbag2_storage_default_plugins/sqlite/sqlite_storage.cpp @@ -224,12 +224,12 @@ void SqliteStorage::prepare_for_reading() void SqliteStorage::fill_topics_and_types() { auto statement = database_->prepare_statement( - "SELECT name, type, serialization_format, offered_qos_profiles FROM topics ORDER BY id;"); - auto query_results = statement->execute_query(); + "SELECT name, type, serialization_format FROM topics ORDER BY id;"); + auto query_results = statement->execute_query(); 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<3>(result)}); + {std::get<0>(result), std::get<1>(result), std::get<2>(result)}); } } @@ -258,12 +258,12 @@ rosbag2_storage::BagMetadata SqliteStorage::get_metadata() metadata.topics_with_message_count = {}; auto statement = database_->prepare_statement( - "SELECT name, type, serialization_format, offered_qos_profiles, " - "COUNT(messages.id), MIN(messages.timestamp), MAX(messages.timestamp) " + "SELECT name, type, serialization_format, COUNT(messages.id), MIN(messages.timestamp), " + "MAX(messages.timestamp) " "FROM messages JOIN topics on topics.id = messages.topic_id " "GROUP BY topics.name;"); auto query_results = statement->execute_query< - std::string, std::string, std::string, std::string, int, rcutils_time_point_value_t, + std::string, std::string, std::string, int, rcutils_time_point_value_t, rcutils_time_point_value_t>(); rcutils_time_point_value_t min_time = INT64_MAX; @@ -271,13 +271,13 @@ rosbag2_storage::BagMetadata SqliteStorage::get_metadata() for (auto result : query_results) { metadata.topics_with_message_count.push_back( { - {std::get<0>(result), std::get<1>(result), std::get<2>(result), std::get<3>(result)}, - static_cast(std::get<4>(result)) + {std::get<0>(result), std::get<1>(result), std::get<2>(result)}, + static_cast(std::get<3>(result)) }); - metadata.message_count += std::get<4>(result); - min_time = std::get<4>(result) < min_time ? std::get<5>(result) : min_time; - max_time = std::get<5>(result) > max_time ? std::get<6>(result) : max_time; + metadata.message_count += std::get<3>(result); + min_time = std::get<4>(result) < min_time ? std::get<4>(result) : min_time; + max_time = std::get<5>(result) > max_time ? std::get<5>(result) : max_time; } if (metadata.message_count == 0) { diff --git a/rosbag2_tests/CMakeLists.txt b/rosbag2_tests/CMakeLists.txt index cc7335451..c27a5eac8 100644 --- a/rosbag2_tests/CMakeLists.txt +++ b/rosbag2_tests/CMakeLists.txt @@ -67,14 +67,6 @@ if(BUILD_TESTING) # test_msgs) # endif() - # ament_add_gmock(test_rosbag2_qos - # test/rosbag2_tests/test_rosbag2_qos.cpp - # WORKING_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}) - # if(TARGET test_rosbag2_qos) - # ament_target_dependencies(test_rosbag2_qos - # rosbag2_transport) - # endif() - ament_add_gmock(test_rosbag2_info_end_to_end test/rosbag2_tests/test_rosbag2_info_end_to_end.cpp WORKING_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}) diff --git a/rosbag2_tests/test/rosbag2_tests/test_rosbag2_qos.cpp b/rosbag2_tests/test/rosbag2_tests/test_rosbag2_qos.cpp deleted file mode 100644 index f683e4427..000000000 --- a/rosbag2_tests/test/rosbag2_tests/test_rosbag2_qos.cpp +++ /dev/null @@ -1,46 +0,0 @@ -#include - -#include "rosbag2_transport/play_options.hpp" -#include "rosbag2_transport/storage_options.hpp" - -namespace -{} - -TEST() { - rosbag2_transport::PlayOptions play_options{}; - play_options.node_prefix = ""; - play_options.read_ahead_queue_size = read_ahead_queue_size; - - rosbag2_transport::StorageOptions storage_options{}; - storage_options.uri = ""; - storage_options.storage_id = ""; - - size_t read_ahead_queue_size; - - rosbag2_storage::MetadataIo metadata_io{}; - rosbag2_storage::BagMetadata metadata{}; - // Specify defaults - auto info = std::make_shared(); - std::shared_ptr reader; - auto writer = std::make_shared( - std::make_unique()); - // Change reader based on metadata options - if (metadata_io.metadata_file_exists(storage_options.uri)) { - metadata = metadata_io.read_metadata(storage_options.uri); - if (metadata.compression_format == "zstd") { - reader = std::make_shared( - std::make_unique()); - } else { - reader = std::make_shared( - std::make_unique()); - } - } else { - reader = std::make_shared( - std::make_unique()); - } - - rosbag2_transport::Rosbag2Transport transport(reader, writer, info); - transport.init(); - transport.play(storage_options, play_options); - transport.shutdown(); -} diff --git a/rosbag2_transport/test/rosbag2_transport/test_formatter.cpp b/rosbag2_transport/test/rosbag2_transport/test_formatter.cpp index 47fdbdd68..a43bf98bd 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_formatter.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_formatter.cpp @@ -74,8 +74,8 @@ TEST_F(FormatterTestFixture, format_files_prints_newline_if_there_are_no_paths) TEST_F(FormatterTestFixture, format_topics_with_type_correctly_layouts_more_topics) { std::vector topics; - topics.push_back({{"topic1", "type1", "rmw1", "qos_profile1"}, 100}); - topics.push_back({{"topic2", "type2", "rmw2", "qos_profile2"}, 200}); + topics.push_back({{"topic1", "type1", "rmw1", ""}, 100}); + topics.push_back({{"topic2", "type2", "rmw2", ""}, 200}); std::stringstream formatted_output; formatter_->format_topics_with_type(topics, formatted_output, indentation_spaces_); diff --git a/rosbag2_transport/test/rosbag2_transport/test_info.cpp b/rosbag2_transport/test/rosbag2_transport/test_info.cpp index 0f8f2ef17..94829111b 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_info.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_info.cpp @@ -36,8 +36,8 @@ TEST_F(Rosbag2TransportTestFixture, info_pretty_prints_information_from_bagfile) std::chrono::nanoseconds(1538051985348887471)); // corresponds to Sept 27 14:39:45.348 bagfile.duration = std::chrono::nanoseconds(50000000); bagfile.message_count = 50; - bagfile.topics_with_message_count.push_back({{"topic1", "type1", "rmw1", "qos_profile1"}, 100}); - bagfile.topics_with_message_count.push_back({{"topic2", "type2", "rmw2", "qos_profile2"}, 200}); + bagfile.topics_with_message_count.push_back({{"topic1", "type1", "rmw1", ""}, 100}); + bagfile.topics_with_message_count.push_back({{"topic2", "type2", "rmw2", ""}, 200}); EXPECT_CALL(*info_, read_metadata(_, _)).WillOnce(Return(bagfile)); // the expected output uses a regex to handle different time zones. From 98f2ddce5a98b7e76b6bbb7d53ec9e30c31d96c6 Mon Sep 17 00:00:00 2001 From: Emerson Knapp Date: Wed, 25 Mar 2020 15:14:42 -0700 Subject: [PATCH 06/15] Bump metadata version Signed-off-by: Emerson Knapp --- rosbag2_storage/include/rosbag2_storage/bag_metadata.hpp | 2 +- rosbag2_storage/src/rosbag2_storage/metadata_io.cpp | 1 + 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/rosbag2_storage/include/rosbag2_storage/bag_metadata.hpp b/rosbag2_storage/include/rosbag2_storage/bag_metadata.hpp index 706b30dd2..9f2ffd13a 100644 --- a/rosbag2_storage/include/rosbag2_storage/bag_metadata.hpp +++ b/rosbag2_storage/include/rosbag2_storage/bag_metadata.hpp @@ -33,7 +33,7 @@ struct TopicInformation struct BagMetadata { - int version = 3; // upgrade this number when changing the content of the struct + int version = 4; // 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/src/rosbag2_storage/metadata_io.cpp b/rosbag2_storage/src/rosbag2_storage/metadata_io.cpp index 4da663c98..8806022de 100644 --- a/rosbag2_storage/src/rosbag2_storage/metadata_io.cpp +++ b/rosbag2_storage/src/rosbag2_storage/metadata_io.cpp @@ -57,6 +57,7 @@ struct convert topic.name = node["name"].as(); topic.type = node["type"].as(); topic.serialization_format = node["serialization_format"].as(); + return true; } }; From 84ac660d9740a19353fa5cea6aeb77d12ba69eef Mon Sep 17 00:00:00 2001 From: Emerson Knapp Date: Wed, 25 Mar 2020 15:34:49 -0700 Subject: [PATCH 07/15] Fix build Signed-off-by: Emerson Knapp --- rosbag2_cpp/test/rosbag2_cpp/test_info.cpp | 6 ++---- .../sqlite/sqlite_storage.cpp | 4 ++-- 2 files changed, 4 insertions(+), 6 deletions(-) diff --git a/rosbag2_cpp/test/rosbag2_cpp/test_info.cpp b/rosbag2_cpp/test/rosbag2_cpp/test_info.cpp index 0f4686803..21891bf1c 100644 --- a/rosbag2_cpp/test/rosbag2_cpp/test_info.cpp +++ b/rosbag2_cpp/test/rosbag2_cpp/test_info.cpp @@ -148,8 +148,7 @@ TEST_F(TemporaryDirectoryFixture, read_metadata_makes_appropriate_call_to_metada EXPECT_THAT(read_metadata.message_count, Eq(50u)); 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}; + rosbag2_storage::TopicInformation expected_first_topic = {{"topic1", "type1", "rmw1", ""}, 100}; EXPECT_THAT( actual_first_topic.topic_metadata.name, Eq(expected_first_topic.topic_metadata.name)); @@ -161,8 +160,7 @@ TEST_F(TemporaryDirectoryFixture, read_metadata_makes_appropriate_call_to_metada Eq(expected_first_topic.topic_metadata.serialization_format)); 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}; + rosbag2_storage::TopicInformation expected_second_topic = {{"topic2", "type2", "rmw2", ""}, 200}; EXPECT_THAT( actual_second_topic.topic_metadata.name, Eq(expected_second_topic.topic_metadata.name)); diff --git a/rosbag2_storage_default_plugins/src/rosbag2_storage_default_plugins/sqlite/sqlite_storage.cpp b/rosbag2_storage_default_plugins/src/rosbag2_storage_default_plugins/sqlite/sqlite_storage.cpp index dd90a045d..2d9306a83 100644 --- a/rosbag2_storage_default_plugins/src/rosbag2_storage_default_plugins/sqlite/sqlite_storage.cpp +++ b/rosbag2_storage_default_plugins/src/rosbag2_storage_default_plugins/sqlite/sqlite_storage.cpp @@ -229,7 +229,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), {}}); } } @@ -271,7 +271,7 @@ rosbag2_storage::BagMetadata SqliteStorage::get_metadata() for (auto result : query_results) { metadata.topics_with_message_count.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), {}}, static_cast(std::get<3>(result)) }); From 99e8ed36cd2bcf7e754ffd03b8f076f61cad9f37 Mon Sep 17 00:00:00 2001 From: Emerson Knapp Date: Wed, 25 Mar 2020 15:53:59 -0700 Subject: [PATCH 08/15] Move yaml conversions into cpp Signed-off-by: Emerson Knapp --- .../src/rosbag2_storage/metadata_io.cpp | 13 +--- rosbag2_transport/CMakeLists.txt | 1 + .../src/rosbag2_transport/qos.cpp | 52 +++++++++++++++ .../src/rosbag2_transport/qos.hpp | 64 +++---------------- .../src/rosbag2_transport/yaml.hpp | 13 ++++ 5 files changed, 75 insertions(+), 68 deletions(-) create mode 100644 rosbag2_transport/src/rosbag2_transport/qos.cpp create mode 100644 rosbag2_transport/src/rosbag2_transport/yaml.hpp diff --git a/rosbag2_storage/src/rosbag2_storage/metadata_io.cpp b/rosbag2_storage/src/rosbag2_storage/metadata_io.cpp index 8806022de..e5d127ca8 100644 --- a/rosbag2_storage/src/rosbag2_storage/metadata_io.cpp +++ b/rosbag2_storage/src/rosbag2_storage/metadata_io.cpp @@ -24,18 +24,7 @@ #include "rosbag2_storage/topic_metadata.hpp" -#ifdef _WIN32 -// This is necessary because of a bug in yaml-cpp's cmake -#define YAML_CPP_DLL -// 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 "yaml.hpp" namespace YAML { diff --git a/rosbag2_transport/CMakeLists.txt b/rosbag2_transport/CMakeLists.txt index d3e03076b..a1bc255e3 100644 --- a/rosbag2_transport/CMakeLists.txt +++ b/rosbag2_transport/CMakeLists.txt @@ -35,6 +35,7 @@ add_library(${PROJECT_NAME} SHARED src/rosbag2_transport/formatter.cpp src/rosbag2_transport/generic_publisher.cpp src/rosbag2_transport/generic_subscription.cpp + src/rosbag2_transport/qos.cpp src/rosbag2_transport/recorder.cpp src/rosbag2_transport/rosbag2_node.cpp src/rosbag2_transport/rosbag2_transport.cpp) diff --git a/rosbag2_transport/src/rosbag2_transport/qos.cpp b/rosbag2_transport/src/rosbag2_transport/qos.cpp new file mode 100644 index 000000000..4fe187878 --- /dev/null +++ b/rosbag2_transport/src/rosbag2_transport/qos.cpp @@ -0,0 +1,52 @@ +#include "qos.hpp" + +namespace YAML +{ +Node convert::encode(const rmw_time_t & time) +{ + Node node; + node["sec"] = time.sec; + node["nsec"] = time.nsec; + return node; +} + +bool convert::decode(const Node & node, rmw_time_t & time) +{ + time.sec = node["sec"].as(); + time.nsec = node["nsec"].as(); + return true; +} + +Node convert::encode(const rosbag2_transport::Rosbag2QoS & qos) +{ + const auto & p = qos.get_rmw_qos_profile(); + Node node; + node["history"] = (int)p.history; + node["depth"] = p.depth; + node["reliability"] = (int)p.reliability; + node["durability"] = (int)p.durability; + node["deadline"] = p.deadline; + node["lifespan"] = p.lifespan; + node["liveliness"] = (int)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) +{ + qos + .keep_last(node["depth"].as()) + .history((rmw_qos_history_policy_t)node["history"].as()) + .reliability((rmw_qos_reliability_policy_t)node["reliability"].as()) + .durability((rmw_qos_durability_policy_t)node["durability"].as()) + .deadline(node["deadline"].as()) + .lifespan(node["lifespan"].as()) + .liveliness((rmw_qos_liveliness_policy_t)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 diff --git a/rosbag2_transport/src/rosbag2_transport/qos.hpp b/rosbag2_transport/src/rosbag2_transport/qos.hpp index 5c9d3834d..57c852642 100644 --- a/rosbag2_transport/src/rosbag2_transport/qos.hpp +++ b/rosbag2_transport/src/rosbag2_transport/qos.hpp @@ -17,26 +17,17 @@ #include "rclcpp/qos.hpp" -#ifdef _WIN32 -// This is necessary because of a bug in yaml-cpp's cmake -#define YAML_CPP_DLL -// 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 "yaml.hpp" + namespace rosbag2_transport { +/// Simple wrapper around rclcpp::QoS to provide a default constructor for YAML deserialization. class Rosbag2QoS : public rclcpp::QoS { public: explicit Rosbag2QoS() : rclcpp::QoS(10) {} - explicit Rosbag2QoS(const rclcpp::QoS & other) : rclcpp::QoS(other) {} + explicit Rosbag2QoS(const rclcpp::QoS & value) : rclcpp::QoS(value) {} }; } // namespace rosbag2_transport @@ -46,54 +37,15 @@ namespace YAML template<> struct convert { - static Node encode(const rmw_time_t & time) - { - Node node; - node["sec"] = time.sec; - node["nsec"] = time.nsec; - return node; - } - static bool decode(const Node & node, rmw_time_t & time) - { - time.sec = node["sec"].as(); - time.nsec = node["nsec"].as(); - return true; - } + static Node encode(const rmw_time_t & time); + static bool decode(const Node & node, rmw_time_t & time); }; template<> struct convert { - static Node encode(const rosbag2_transport::Rosbag2QoS & qos) - { - const auto & p = qos.get_rmw_qos_profile(); - Node node; - node["history"] = (int)p.history; - node["depth"] = p.depth; - node["reliability"] = (int)p.reliability; - node["durability"] = (int)p.durability; - node["deadline"] = p.deadline; - node["lifespan"] = p.lifespan; - node["liveliness"] = (int)p.liveliness; - node["liveliness_lease_duration"] = p.liveliness_lease_duration; - node["avoid_ros_namespace_conventions"] = p.avoid_ros_namespace_conventions; - return node; - } - static bool decode(const Node & node, rosbag2_transport::Rosbag2QoS & qos) - { - qos - .keep_last(node["depth"].as()) - .history((rmw_qos_history_policy_t)node["history"].as()) - .reliability((rmw_qos_reliability_policy_t)node["reliability"].as()) - .durability((rmw_qos_durability_policy_t)node["durability"].as()) - .deadline(node["deadline"].as()) - .lifespan(node["lifespan"].as()) - .liveliness((rmw_qos_liveliness_policy_t)node["liveliness"].as()) - .liveliness_lease_duration(node["liveliness_lease_duration"].as()) - .avoid_ros_namespace_conventions(node["avoid_ros_namespace_conventions"].as()) - ; - return true; - } + static Node encode(const rosbag2_transport::Rosbag2QoS & qos); + static bool decode(const Node & node, rosbag2_transport::Rosbag2QoS & qos); }; } // namespace YAML diff --git a/rosbag2_transport/src/rosbag2_transport/yaml.hpp b/rosbag2_transport/src/rosbag2_transport/yaml.hpp new file mode 100644 index 000000000..b4f844db6 --- /dev/null +++ b/rosbag2_transport/src/rosbag2_transport/yaml.hpp @@ -0,0 +1,13 @@ + +#ifdef _WIN32 +// This is necessary because of a bug in yaml-cpp's cmake +#define YAML_CPP_DLL +// 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 From aa9d0a4d791d2a58eccea21cb4a5c0880938cfe9 Mon Sep 17 00:00:00 2001 From: Emerson Knapp Date: Wed, 25 Mar 2020 17:21:07 -0700 Subject: [PATCH 09/15] Fix linters Signed-off-by: Emerson Knapp --- .../src/rosbag2_storage/metadata_io.cpp | 13 +++++- .../sqlite/sqlite_storage.cpp | 6 ++- .../src/rosbag2_transport/qos.cpp | 46 +++++++++++++------ .../src/rosbag2_transport/qos.hpp | 21 +++++++-- .../src/rosbag2_transport/recorder.cpp | 5 +- .../src/rosbag2_transport/yaml.hpp | 13 ------ 6 files changed, 67 insertions(+), 37 deletions(-) delete mode 100644 rosbag2_transport/src/rosbag2_transport/yaml.hpp diff --git a/rosbag2_storage/src/rosbag2_storage/metadata_io.cpp b/rosbag2_storage/src/rosbag2_storage/metadata_io.cpp index e5d127ca8..8806022de 100644 --- a/rosbag2_storage/src/rosbag2_storage/metadata_io.cpp +++ b/rosbag2_storage/src/rosbag2_storage/metadata_io.cpp @@ -24,7 +24,18 @@ #include "rosbag2_storage/topic_metadata.hpp" -#include "yaml.hpp" +#ifdef _WIN32 +// This is necessary because of a bug in yaml-cpp's cmake +#define YAML_CPP_DLL +// 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 namespace YAML { diff --git a/rosbag2_storage_default_plugins/src/rosbag2_storage_default_plugins/sqlite/sqlite_storage.cpp b/rosbag2_storage_default_plugins/src/rosbag2_storage_default_plugins/sqlite/sqlite_storage.cpp index 2d9306a83..b606edafc 100644 --- a/rosbag2_storage_default_plugins/src/rosbag2_storage_default_plugins/sqlite/sqlite_storage.cpp +++ b/rosbag2_storage_default_plugins/src/rosbag2_storage_default_plugins/sqlite/sqlite_storage.cpp @@ -185,8 +185,10 @@ void SqliteStorage::create_topic(const rosbag2_storage::TopicMetadata & topic) if (topics_.find(topic.name) == std::end(topics_)) { auto insert_topic = database_->prepare_statement( - "INSERT INTO topics (name, type, serialization_format, offered_qos_profiles) VALUES (?, ?, ?, ?)"); - insert_topic->bind(topic.name, topic.type, topic.serialization_format, topic.offered_qos_profiles); + "INSERT INTO topics (name, type, serialization_format, offered_qos_profiles) " + "VALUES (?, ?, ?, ?)"); + insert_topic->bind( + topic.name, topic.type, topic.serialization_format, topic.offered_qos_profiles); insert_topic->execute_and_reset(); topics_.emplace(topic.name, static_cast(database_->get_last_insert_id())); } diff --git a/rosbag2_transport/src/rosbag2_transport/qos.cpp b/rosbag2_transport/src/rosbag2_transport/qos.cpp index 4fe187878..00d2c5cbb 100644 --- a/rosbag2_transport/src/rosbag2_transport/qos.cpp +++ b/rosbag2_transport/src/rosbag2_transport/qos.cpp @@ -1,3 +1,17 @@ +// Copyright 2020 Amazon.com, Inc. or its affiliates. All Rights Reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + #include "qos.hpp" namespace YAML @@ -21,13 +35,13 @@ Node convert::encode(const rosbag2_transport::Ros { const auto & p = qos.get_rmw_qos_profile(); Node node; - node["history"] = (int)p.history; + node["history"] = static_cast(p.history); node["depth"] = p.depth; - node["reliability"] = (int)p.reliability; - node["durability"] = (int)p.durability; + node["reliability"] = static_cast(p.reliability); + node["durability"] = static_cast(p.durability); node["deadline"] = p.deadline; node["lifespan"] = p.lifespan; - node["liveliness"] = (int)p.liveliness; + 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; @@ -36,17 +50,21 @@ 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((rmw_qos_history_policy_t)node["history"].as()) - .reliability((rmw_qos_reliability_policy_t)node["reliability"].as()) - .durability((rmw_qos_durability_policy_t)node["durability"].as()) - .deadline(node["deadline"].as()) - .lifespan(node["lifespan"].as()) - .liveliness((rmw_qos_liveliness_policy_t)node["liveliness"].as()) - .liveliness_lease_duration(node["liveliness_lease_duration"].as()) - .avoid_ros_namespace_conventions(node["avoid_ros_namespace_conventions"].as()) - ; + .keep_last(node["depth"].as()) + .history(history) + .reliability(reliability) + .durability(durability) + .deadline(node["deadline"].as()) + .lifespan(node["lifespan"].as()) + .liveliness(liveliness) + .liveliness_lease_duration(node["liveliness_lease_duration"].as()) + .avoid_ros_namespace_conventions(node["avoid_ros_namespace_conventions"].as()); return true; } } // namespace YAML diff --git a/rosbag2_transport/src/rosbag2_transport/qos.hpp b/rosbag2_transport/src/rosbag2_transport/qos.hpp index 57c852642..45a4c46a1 100644 --- a/rosbag2_transport/src/rosbag2_transport/qos.hpp +++ b/rosbag2_transport/src/rosbag2_transport/qos.hpp @@ -17,7 +17,18 @@ #include "rclcpp/qos.hpp" -#include "yaml.hpp" +#ifdef _WIN32 +// This is necessary because of a bug in yaml-cpp's cmake +#define YAML_CPP_DLL +// 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 namespace rosbag2_transport @@ -26,8 +37,10 @@ namespace rosbag2_transport class Rosbag2QoS : public rclcpp::QoS { public: - explicit Rosbag2QoS() : rclcpp::QoS(10) {} - explicit Rosbag2QoS(const rclcpp::QoS & value) : rclcpp::QoS(value) {} + Rosbag2QoS() + : rclcpp::QoS(10) {} + explicit Rosbag2QoS(const rclcpp::QoS & value) + : rclcpp::QoS(value) {} }; } // namespace rosbag2_transport @@ -50,4 +63,4 @@ struct convert } // namespace YAML -#endif +#endif // ROSBAG2_TRANSPORT__QOS_HPP_ diff --git a/rosbag2_transport/src/rosbag2_transport/recorder.cpp b/rosbag2_transport/src/rosbag2_transport/recorder.cpp index c815b84de..ddf63fea4 100644 --- a/rosbag2_transport/src/rosbag2_transport/recorder.cpp +++ b/rosbag2_transport/src/rosbag2_transport/recorder.cpp @@ -144,12 +144,11 @@ void Recorder::subscribe_topic(const rosbag2_storage::TopicMetadata & topic_with subscription_qos = last_qos; ROSBAG2_TRANSPORT_LOG_INFO_STREAM( "OK! All publishers for topic " << topic.name << - " offering the same QoS profile - using it to subscribe."); + " offering the same QoS profile - using it to subscribe."); } else { ROSBAG2_TRANSPORT_LOG_WARN_STREAM( "Topic " << topic.name << " has publishers offering different QoS settings. " - "Can't guess what QoS to request, falling back to default QoS profile." - ); + "Can't guess what QoS to request, falling back to default QoS profile."); } // Need to create topic in writer before we are trying to create subscription. Since in diff --git a/rosbag2_transport/src/rosbag2_transport/yaml.hpp b/rosbag2_transport/src/rosbag2_transport/yaml.hpp deleted file mode 100644 index b4f844db6..000000000 --- a/rosbag2_transport/src/rosbag2_transport/yaml.hpp +++ /dev/null @@ -1,13 +0,0 @@ - -#ifdef _WIN32 -// This is necessary because of a bug in yaml-cpp's cmake -#define YAML_CPP_DLL -// 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 From 7ba55852fb868a68cc4fdddda7eb53996f345439 Mon Sep 17 00:00:00 2001 From: Emerson Knapp Date: Wed, 25 Mar 2020 18:20:24 -0700 Subject: [PATCH 10/15] Remove query functionality and add some tests Signed-off-by: Emerson Knapp --- .../test_metadata_serialization.cpp | 10 ++- rosbag2_transport/CMakeLists.txt | 12 +++ .../src/rosbag2_transport/recorder.cpp | 37 +-------- .../test/rosbag2_transport/test_qos.cpp | 76 +++++++++++++++++++ 4 files changed, 97 insertions(+), 38 deletions(-) create mode 100644 rosbag2_transport/test/rosbag2_transport/test_qos.cpp diff --git a/rosbag2_storage/test/rosbag2_storage/test_metadata_serialization.cpp b/rosbag2_storage/test/rosbag2_storage/test_metadata_serialization.cpp index 63c268637..42eb02d90 100644 --- a/rosbag2_storage/test/rosbag2_storage/test_metadata_serialization.cpp +++ b/rosbag2_storage/test/rosbag2_storage/test_metadata_serialization.cpp @@ -53,8 +53,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", ""}, 100}); - metadata.topics_with_message_count.push_back({{"topic2", "type2", "rmw2", ""}, 200}); + 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_io_->write_metadata(temporary_dir_path_, metadata); auto read_metadata = metadata_io_->read_metadata(temporary_dir_path_); @@ -92,4 +92,10 @@ TEST_F(MetadataFixture, test_writing_and_reading_yaml) actual_second_topic.topic_metadata.serialization_format, Eq(expected_second_topic.topic_metadata.serialization_format)); EXPECT_THAT(actual_second_topic.message_count, Eq(expected_second_topic.message_count)); + EXPECT_EQ( + actual_first_topic.topic_metadata.offered_qos_profiles, + expected_first_topic.topic_metadata.offered_qos_profiles); + EXPECT_EQ( + actual_second_topic.topic_metadata.offered_qos_profiles, + expected_second_topic.topic_metadata.offered_qos_profiles); } diff --git a/rosbag2_transport/CMakeLists.txt b/rosbag2_transport/CMakeLists.txt index a1bc255e3..52ad9d8f6 100644 --- a/rosbag2_transport/CMakeLists.txt +++ b/rosbag2_transport/CMakeLists.txt @@ -177,6 +177,18 @@ if(BUILD_TESTING) ament_target_dependencies(test_play test_msgs rosbag2_test_common) endif() + ament_add_gmock(test_qos + test/rosbag2_transport/test_qos.cpp + WORKING_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR} + ${SKIP_TEST}) + if (TARGET test_qos) + target_link_libraries(test_qos rosbag2_transport) + target_include_directories(test_qos + PUBLIC + $) + ament_target_dependencies(test_qos rosbag2_test_common) + endif() + endif() ament_package() diff --git a/rosbag2_transport/src/rosbag2_transport/recorder.cpp b/rosbag2_transport/src/rosbag2_transport/recorder.cpp index ddf63fea4..b4669d63a 100644 --- a/rosbag2_transport/src/rosbag2_transport/recorder.cpp +++ b/rosbag2_transport/src/rosbag2_transport/recorder.cpp @@ -114,43 +114,8 @@ void Recorder::subscribe_topics( } } -void Recorder::subscribe_topic(const rosbag2_storage::TopicMetadata & topic_without_qos) +void Recorder::subscribe_topic(const rosbag2_storage::TopicMetadata & topic) { - rosbag2_storage::TopicMetadata topic { - topic_without_qos.name, - topic_without_qos.type, - topic_without_qos.serialization_format, - ""}; - rclcpp::QoS last_qos(10); - bool first = true; - bool all_qos_same = true; - rclcpp::QoS subscription_qos(10); - - auto publishers_info = node_->get_publishers_info_by_topic(topic.name); - ROSBAG2_TRANSPORT_LOG_ERROR_STREAM("Querying endpoints for topic " << topic.name); - YAML::Node offered_qos_profiles; - for (auto info : publishers_info) { - offered_qos_profiles.push_back(Rosbag2QoS(info.qos_profile())); - if (!first) { - all_qos_same &= last_qos == info.qos_profile(); - } - first = false; - last_qos = info.qos_profile(); - // ROSBAG2_TRANSPORT_LOG_INFO_STREAM("---" << std::endl << last_qos); - } - - topic.offered_qos_profiles = YAML::Dump(offered_qos_profiles); - if (all_qos_same) { - subscription_qos = last_qos; - ROSBAG2_TRANSPORT_LOG_INFO_STREAM( - "OK! All publishers for topic " << topic.name << - " offering the same QoS profile - using it to subscribe."); - } else { - ROSBAG2_TRANSPORT_LOG_WARN_STREAM( - "Topic " << topic.name << " has publishers offering different QoS settings. " - "Can't guess what QoS to request, falling back to default QoS profile."); - } - // Need to create topic in writer before we are trying to create subscription. Since in // callback for subscription we are calling writer_->write(bag_message); and it could happened // that callback called before we reached out the line: writer_->create_topic(topic) diff --git a/rosbag2_transport/test/rosbag2_transport/test_qos.cpp b/rosbag2_transport/test/rosbag2_transport/test_qos.cpp new file mode 100644 index 000000000..1c3b603f2 --- /dev/null +++ b/rosbag2_transport/test/rosbag2_transport/test_qos.cpp @@ -0,0 +1,76 @@ +// Copyright 2020 Amazon.com, Inc. or its affiliates. All Rights Reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include "qos.hpp" + +TEST(TestQoS, serialization) +{ + 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); + printf("%s\n", serialized.c_str()); + + YAML::Node loaded_node = YAML::Load(serialized); + auto deserialized_profiles = loaded_node.as >(); + ASSERT_EQ(deserialized_profiles.size(), 1u); + + rosbag2_transport::Rosbag2QoS actual_qos = deserialized_profiles[0]; + EXPECT_EQ(actual_qos, expected_qos); +} + +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" + " depth: 10\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"; + + YAML::Node loaded_node = YAML::Load(serialized_profiles); + auto deserialized_profiles = loaded_node.as >(); + ASSERT_EQ(deserialized_profiles.size(), 1u); + rosbag2_transport::Rosbag2QoS actual_qos = deserialized_profiles[0]; + + rmw_time_t zerotime{0, 0}; + // Explicitly set up the same QoS profile in case defaults change + rclcpp::QoS expected_qos(10); + expected_qos + .keep_last(10) + .reliable() + .durability_volatile() + .deadline(zerotime) + .lifespan(zerotime) + .liveliness(RMW_QOS_POLICY_LIVELINESS_SYSTEM_DEFAULT) + .liveliness_lease_duration(zerotime) + .avoid_ros_namespace_conventions(false); + // Any values not present in the YAML should take the default value in both profiles + EXPECT_EQ(actual_qos, expected_qos); +} From 1fe86ce262da073e1c8bf260f6709db8006134bb Mon Sep 17 00:00:00 2001 From: Emerson Knapp Date: Wed, 25 Mar 2020 18:21:14 -0700 Subject: [PATCH 11/15] Not reading metadata yet Signed-off-by: Emerson Knapp --- .../test/rosbag2_storage/test_metadata_serialization.cpp | 6 ------ 1 file changed, 6 deletions(-) diff --git a/rosbag2_storage/test/rosbag2_storage/test_metadata_serialization.cpp b/rosbag2_storage/test/rosbag2_storage/test_metadata_serialization.cpp index 42eb02d90..f13105572 100644 --- a/rosbag2_storage/test/rosbag2_storage/test_metadata_serialization.cpp +++ b/rosbag2_storage/test/rosbag2_storage/test_metadata_serialization.cpp @@ -92,10 +92,4 @@ TEST_F(MetadataFixture, test_writing_and_reading_yaml) actual_second_topic.topic_metadata.serialization_format, Eq(expected_second_topic.topic_metadata.serialization_format)); EXPECT_THAT(actual_second_topic.message_count, Eq(expected_second_topic.message_count)); - EXPECT_EQ( - actual_first_topic.topic_metadata.offered_qos_profiles, - expected_first_topic.topic_metadata.offered_qos_profiles); - EXPECT_EQ( - actual_second_topic.topic_metadata.offered_qos_profiles, - expected_second_topic.topic_metadata.offered_qos_profiles); } From b7150383199629adb63b08cdd9beaa88bb0c95ba Mon Sep 17 00:00:00 2001 From: Emerson Knapp Date: Thu, 26 Mar 2020 11:49:44 -0700 Subject: [PATCH 12/15] Address review comments Signed-off-by: Emerson Knapp --- rosbag2_storage/include/rosbag2_storage/topic_metadata.hpp | 2 +- .../rosbag2_storage_default_plugins/sqlite/sqlite_storage.cpp | 4 ++-- rosbag2_transport/src/rosbag2_transport/qos.hpp | 2 +- rosbag2_transport/src/rosbag2_transport/recorder.cpp | 1 - 4 files changed, 4 insertions(+), 5 deletions(-) diff --git a/rosbag2_storage/include/rosbag2_storage/topic_metadata.hpp b/rosbag2_storage/include/rosbag2_storage/topic_metadata.hpp index 4b3ad9d09..e43fa12fc 100644 --- a/rosbag2_storage/include/rosbag2_storage/topic_metadata.hpp +++ b/rosbag2_storage/include/rosbag2_storage/topic_metadata.hpp @@ -25,7 +25,7 @@ struct TopicMetadata std::string name; std::string type; std::string serialization_format; - // std::vector serialized to YAML string + // Serialized std::vector as a YAML string std::string offered_qos_profiles; bool operator==(const rosbag2_storage::TopicMetadata & rhs) const diff --git a/rosbag2_storage_default_plugins/src/rosbag2_storage_default_plugins/sqlite/sqlite_storage.cpp b/rosbag2_storage_default_plugins/src/rosbag2_storage_default_plugins/sqlite/sqlite_storage.cpp index b606edafc..bdc1e23e2 100644 --- a/rosbag2_storage_default_plugins/src/rosbag2_storage_default_plugins/sqlite/sqlite_storage.cpp +++ b/rosbag2_storage_default_plugins/src/rosbag2_storage_default_plugins/sqlite/sqlite_storage.cpp @@ -231,7 +231,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), ""}); } } @@ -273,7 +273,7 @@ rosbag2_storage::BagMetadata SqliteStorage::get_metadata() for (auto result : query_results) { metadata.topics_with_message_count.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), ""}, static_cast(std::get<3>(result)) }); diff --git a/rosbag2_transport/src/rosbag2_transport/qos.hpp b/rosbag2_transport/src/rosbag2_transport/qos.hpp index 45a4c46a1..de8027f35 100644 --- a/rosbag2_transport/src/rosbag2_transport/qos.hpp +++ b/rosbag2_transport/src/rosbag2_transport/qos.hpp @@ -38,7 +38,7 @@ class Rosbag2QoS : public rclcpp::QoS { public: Rosbag2QoS() - : rclcpp::QoS(10) {} + : rclcpp::QoS(0) {} // 0 history depth is always overwritten on deserializing explicit Rosbag2QoS(const rclcpp::QoS & value) : rclcpp::QoS(value) {} }; diff --git a/rosbag2_transport/src/rosbag2_transport/recorder.cpp b/rosbag2_transport/src/rosbag2_transport/recorder.cpp index b4669d63a..5eab2a239 100644 --- a/rosbag2_transport/src/rosbag2_transport/recorder.cpp +++ b/rosbag2_transport/src/rosbag2_transport/recorder.cpp @@ -29,7 +29,6 @@ #include "generic_subscription.hpp" #include "rosbag2_node.hpp" -#include "qos.hpp" namespace rosbag2_transport { From 5b5caf65a10a8fbc9c36fa9852fdea89cdcc6fa1 Mon Sep 17 00:00:00 2001 From: Emerson Knapp Date: Thu, 26 Mar 2020 11:57:06 -0700 Subject: [PATCH 13/15] Fix linters for test fileO Signed-off-by: Emerson Knapp --- rosbag2_transport/CMakeLists.txt | 2 +- rosbag2_transport/test/rosbag2_transport/test_qos.cpp | 7 +++++-- 2 files changed, 6 insertions(+), 3 deletions(-) diff --git a/rosbag2_transport/CMakeLists.txt b/rosbag2_transport/CMakeLists.txt index 52ad9d8f6..31d9f2ad7 100644 --- a/rosbag2_transport/CMakeLists.txt +++ b/rosbag2_transport/CMakeLists.txt @@ -181,7 +181,7 @@ if(BUILD_TESTING) test/rosbag2_transport/test_qos.cpp WORKING_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR} ${SKIP_TEST}) - if (TARGET test_qos) + if(TARGET test_qos) target_link_libraries(test_qos rosbag2_transport) target_include_directories(test_qos PUBLIC diff --git a/rosbag2_transport/test/rosbag2_transport/test_qos.cpp b/rosbag2_transport/test/rosbag2_transport/test_qos.cpp index 1c3b603f2..702e34fa1 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_qos.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_qos.cpp @@ -14,6 +14,9 @@ #include +#include +#include + #include "qos.hpp" TEST(TestQoS, serialization) @@ -26,7 +29,7 @@ TEST(TestQoS, serialization) printf("%s\n", serialized.c_str()); 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]; @@ -55,7 +58,7 @@ 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 = loaded_node.as>(); ASSERT_EQ(deserialized_profiles.size(), 1u); rosbag2_transport::Rosbag2QoS actual_qos = deserialized_profiles[0]; From aa55d43aa26ede7636dffc61838eb84b6dce928f Mon Sep 17 00:00:00 2001 From: Emerson Knapp Date: Thu, 26 Mar 2020 12:06:21 -0700 Subject: [PATCH 14/15] Add field detector test Signed-off-by: Emerson Knapp --- .../test/rosbag2_transport/test_qos.cpp | 21 +++++++++++++++++++ 1 file changed, 21 insertions(+) diff --git a/rosbag2_transport/test/rosbag2_transport/test_qos.cpp b/rosbag2_transport/test/rosbag2_transport/test_qos.cpp index 702e34fa1..00fdd3eed 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_qos.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_qos.cpp @@ -17,6 +17,8 @@ #include #include +#include "rmw/types.h" + #include "qos.hpp" TEST(TestQoS, serialization) @@ -77,3 +79,22 @@ TEST(TestQoS, supports_version_4) // Any values not present in the YAML should take the default value in both profiles EXPECT_EQ(actual_qos, expected_qos); } + +TEST(TestQoS, detect_new_qos_fields) +{ + // By trying to construct a profile explicitly by fields, the build fails if policies are added + // This build failure indicates that we need to update QoS serialization in rosbag2_transport + rmw_time_t notime{0, 0}; + rmw_qos_profile_t profile{ + RMW_QOS_POLICY_HISTORY_SYSTEM_DEFAULT, + 10, + RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT, + RMW_QOS_POLICY_DURABILITY_SYSTEM_DEFAULT, + notime, + notime, + RMW_QOS_POLICY_LIVELINESS_SYSTEM_DEFAULT, + notime, + false, + }; + EXPECT_EQ(profile.history, RMW_QOS_POLICY_HISTORY_SYSTEM_DEFAULT); // fix "unused variable" +} From d0802b3af95e3a3c9581897fc8889b8b1bc1058e Mon Sep 17 00:00:00 2001 From: Emerson Knapp Date: Thu, 26 Mar 2020 16:00:18 -0700 Subject: [PATCH 15/15] Remove leftover print Signed-off-by: Emerson Knapp --- rosbag2_transport/test/rosbag2_transport/test_qos.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/rosbag2_transport/test/rosbag2_transport/test_qos.cpp b/rosbag2_transport/test/rosbag2_transport/test_qos.cpp index 00fdd3eed..37f2e135e 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_qos.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_qos.cpp @@ -28,8 +28,6 @@ TEST(TestQoS, serialization) offered_qos_profiles.push_back(expected_qos); std::string serialized = YAML::Dump(offered_qos_profiles); - printf("%s\n", serialized.c_str()); - YAML::Node loaded_node = YAML::Load(serialized); auto deserialized_profiles = loaded_node.as>(); ASSERT_EQ(deserialized_profiles.size(), 1u);