Add QoS to metadata (re-do #330) (#335)

* serialize and deserialize QoS in metadata

Signed-off-by: Emerson Knapp <[email protected]>

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<rosbag2_storage::TopicMetadata>{topic_with_type_};
     auto message = std::make_shared<rosbag2_storage::SerializedBagMessage>();
     message->topic_name = topic_with_type_.name;

rosbag2_cpp/test/rosbag2_cpp/test_info.cpp

@@ -82,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"}, 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);
@@ -91,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"}, 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);
@@ -148,7 +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));
@@ -160,7 +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));

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<rosbag2_storage::TopicMetadata>{topic_with_type};
 
     auto message = std::make_shared<rosbag2_storage::SerializedBagMessage>();

rosbag2_cpp/test/rosbag2_cpp/test_sequential_writer.cpp

@@ -93,7 +93,7 @@ TEST_F(
   auto message = std::make_shared<rosbag2_storage::SerializedBagMessage>();
   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<rosbag2_storage::SerializedBagMessage>();
   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);

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{

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<std::string> relative_file_paths;

rosbag2_storage/include/rosbag2_storage/topic_metadata.hpp

@@ -25,6 +25,8 @@ struct TopicMetadata
   std::string name;
   std::string type;
   std::string serialization_format;
+  // Serialized std::vector<rclcpp::QoS> as a YAML string
+  std::string offered_qos_profiles;
 
   bool operator==(const rosbag2_storage::TopicMetadata & rhs) const
   {

rosbag2_storage/src/rosbag2_storage/metadata_io.cpp

@@ -48,6 +48,7 @@ struct convert<rosbag2_storage::TopicMetadata>
     node["name"] = topic.name;
     node["type"] = topic.type;
     node["serialization_format"] = topic.serialization_format;
+    node["offered_qos_profiles"] = topic.offered_qos_profiles;
     return node;
   }
 

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::high_resolution_clock>(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_);

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," \
@@ -184,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) 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<int>(database_->get_last_insert_id()));
   }
@@ -228,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), ""});
   }
 }
 
@@ -270,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<size_t>(std::get<3>(result))
       });
 

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<rosbag2_storage::SerializedBagMessage>();
       bag_message->serialized_data = make_serialized_message(std::get<0>(msg));
       bag_message->time_stamp = std::get<1>(msg);

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();
 

rosbag2_transport/CMakeLists.txt

@@ -29,12 +29,14 @@ find_package(rmw REQUIRED)
 find_package(rosbag2_compression REQUIRED)
 find_package(rosbag2_cpp REQUIRED)
 find_package(shared_queues_vendor REQUIRED)
+find_package(yaml_cpp_vendor REQUIRED)
 
 add_library(${PROJECT_NAME} SHARED
   src/rosbag2_transport/player.cpp
   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)
@@ -59,7 +61,7 @@ add_library(rosbag2_transport_py
   src/rosbag2_transport/rosbag2_transport_python.cpp)
 configure_python_c_extension_library(rosbag2_transport_py)
 target_link_libraries(rosbag2_transport_py rosbag2_transport)
-ament_target_dependencies(rosbag2_transport_py rosbag2_compression)
+ament_target_dependencies(rosbag2_transport_py rosbag2_compression yaml_cpp_vendor)
 # Causes the visibility macros to use dllexport rather than dllimport,
 # which is appropriate when building the dll but not consuming it.
 target_compile_definitions(${PROJECT_NAME} PRIVATE "ROSBAG2_TRANSPORT_BUILDING_LIBRARY")
@@ -79,7 +81,7 @@ install(
 ament_export_include_directories(include)
 ament_export_interfaces(export_${PROJECT_NAME})
 ament_export_libraries(${PROJECT_NAME})
-ament_export_dependencies(rosbag2_cpp rosbag2_compression)
+ament_export_dependencies(rosbag2_cpp rosbag2_compression yaml_cpp_vendor)
 
 if(BUILD_TESTING)
   find_package(ament_cmake_gmock REQUIRED)
@@ -176,6 +178,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
+      $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/src/rosbag2_transport>)
+    ament_target_dependencies(test_qos rosbag2_test_common)
+  endif()
+
 endif()
 
 ament_package()

rosbag2_transport/package.xml

@@ -16,6 +16,7 @@
   <depend>rosbag2_cpp</depend>
   <depend>rmw</depend>
   <depend>shared_queues_vendor</depend>
+  <depend>yaml_cpp_vendor</depend>
 
   <test_depend>ament_cmake_gmock</test_depend>
   <test_depend>ament_index_cpp</test_depend>

rosbag2_transport/src/rosbag2_transport/qos.cpp

@@ -0,0 +1,70 @@
+// 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
+{
+Node convert<rmw_time_t>::encode(const rmw_time_t & time)
+{
+  Node node;
+  node["sec"] = time.sec;
+  node["nsec"] = time.nsec;
+  return node;
+}
+
+bool convert<rmw_time_t>::decode(const Node & node, rmw_time_t & time)
+{
+  time.sec = node["sec"].as<uint64_t>();
+  time.nsec = node["nsec"].as<uint64_t>();
+  return true;
+}
+
+Node convert<rosbag2_transport::Rosbag2QoS>::encode(const rosbag2_transport::Rosbag2QoS & qos)
+{
+  const auto & p = qos.get_rmw_qos_profile();
+  Node node;
+  node["history"] = static_cast<int>(p.history);
+  node["depth"] = p.depth;
+  node["reliability"] = static_cast<int>(p.reliability);
+  node["durability"] = static_cast<int>(p.durability);
+  node["deadline"] = p.deadline;
+  node["lifespan"] = p.lifespan;
+  node["liveliness"] = static_cast<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<rosbag2_transport::Rosbag2QoS>::decode(
+  const Node & node, rosbag2_transport::Rosbag2QoS & qos)
+{
+  auto history = static_cast<rmw_qos_history_policy_t>(node["history"].as<int>());
+  auto reliability = static_cast<rmw_qos_reliability_policy_t>(node["reliability"].as<int>());
+  auto durability = static_cast<rmw_qos_durability_policy_t>(node["durability"].as<int>());
+  auto liveliness = static_cast<rmw_qos_liveliness_policy_t>(node["liveliness"].as<int>());
+
+  qos
+  .keep_last(node["depth"].as<int>())
+  .history(history)
+  .reliability(reliability)
+  .durability(durability)
+  .deadline(node["deadline"].as<rmw_time_t>())
+  .lifespan(node["lifespan"].as<rmw_time_t>())
+  .liveliness(liveliness)
+  .liveliness_lease_duration(node["liveliness_lease_duration"].as<rmw_time_t>())
+  .avoid_ros_namespace_conventions(node["avoid_ros_namespace_conventions"].as<bool>());
+  return true;
+}
+}  // namespace YAML