WIP: Set default QoS Profile - Record

ros2bag/ros2bag/verb/record.py

@@ -16,8 +16,8 @@
 import os
 
 from ros2bag.verb import VerbExtension
-
 from ros2cli.node import NODE_NAME_PREFIX
+import yaml
 
 
 class RecordVerb(VerbExtension):
@@ -72,6 +72,10 @@ def add_arguments(self, parser, cli_name):  # noqa: D102
         parser.add_argument(
             '--include-hidden-topics', action='store_true',
             help='record also hidden topics.')
+        parser.add_argument(
+            '--qos-profiles', type=str, default='',
+            help='Path to a yaml file with a QoS policy to override the default profile.'
+        )
         self._subparser = parser
 
     def create_bag_directory(self, uri):
@@ -80,6 +84,27 @@ def create_bag_directory(self, uri):
         except OSError:
             return "[ERROR] [ros2bag]: Could not create bag folder '{}'.".format(uri)
 
+    def validate_qos_profiles(self, qos_profile_path):
+        """Validate the QoS profile yaml file path and its structure."""
+        if os.path.isfile(qos_profile_path):
+            with open(qos_profile_path, 'r') as file:
+                # Load as a dict first to verify contents
+                qos_profile = yaml.safe_load(file)
+                if not qos_profile.get('fallback'):
+                    raise ValueError(
+                        "[ERROR] [ros2bag]: QoS override must include a 'fallback' profile")
+                topic_names = list(qos_profile)
+                for name in topic_names:
+                    if type(qos_profile.get(name)) != dict:
+                        raise ValueError(
+                            '[ERROR] [ros2bag]: QoS profile configuration for topic {} is invalid.'
+                            .format(name))
+                # Read file as a string
+                file.seek(0)
+                return file.read()
+        else:
+            raise ValueError('[ERROR] [ros2bag]: {} does not exist.'.format(qos_profile_path))
+
     def main(self, *, args):  # noqa: D102
         if args.all and args.topics:
             return 'Invalid choice: Can not specify topics and -a at the same time.'
@@ -93,6 +118,13 @@ def main(self, *, args):  # noqa: D102
             return 'Invalid choice: Cannot specify compression format without a compression mode.'
         args.compression_mode = args.compression_mode.upper()
 
+        qos_profiles = args.qos_profiles
+        if qos_profiles:
+            try:
+                qos_profiles = self.validate_qos_profiles(qos_profiles)
+            except ValueError as e:
+                return str(e)
+
         self.create_bag_directory(uri)
 
         if args.all:
@@ -115,7 +147,9 @@ def main(self, *, args):  # noqa: D102
                 polling_interval=args.polling_interval,
                 max_bagfile_size=args.max_bag_size,
                 max_cache_size=args.max_cache_size,
-                include_hidden_topics=args.include_hidden_topics)
+                include_hidden_topics=args.include_hidden_topics,
+                qos_profiles=qos_profiles)
+
         elif args.topics and len(args.topics) > 0:
             # NOTE(hidmic): in merged install workspaces on Windows, Python entrypoint lookups
             #               combined with constrained environments (as imposed by colcon test)
@@ -136,7 +170,8 @@ def main(self, *, args):  # noqa: D102
                 max_bagfile_size=args.max_bag_size,
                 max_cache_size=args.max_cache_size,
                 topics=args.topics,
-                include_hidden_topics=args.include_hidden_topics)
+                include_hidden_topics=args.include_hidden_topics,
+                qos_profiles=qos_profiles)
         else:
             self._subparser.print_help()
 

rosbag2_test_common/include/rosbag2_test_common/publisher_manager.hpp

@@ -75,13 +75,16 @@ class PublisherManager
 
   template<class T>
   void add_publisher(
-    const std::string & topic_name, std::shared_ptr<T> message, size_t expected_messages = 0)
+    const std::string & topic_name,
+    std::shared_ptr<T> message,
+    size_t expected_messages = 0,
+    rclcpp::QoS qos = rclcpp::SystemDefaultsQoS())
   {
     auto node_name = std::string("publisher") + std::to_string(counter_++);
     auto publisher_node = std::make_shared<rclcpp::Node>(
       node_name,
       rclcpp::NodeOptions().start_parameter_event_publisher(false).enable_rosout(false));
-    auto publisher = publisher_node->create_publisher<T>(topic_name, 10);
+    auto publisher = publisher_node->create_publisher<T>(topic_name, qos);
 
     publisher_nodes_.push_back(publisher_node);
     publishers_.push_back(

rosbag2_tests/resources/qos_test/qos_profile.yaml

@@ -0,0 +1,18 @@
+fallback:
+  history: 0
+test_topic:
+  history: 3
+  depth: 0
+  reliability: 1
+  durability: 2
+  deadline:
+    sec: 2147483647
+    nsec: 4294967295
+  lifespan:
+    sec: 2147483647
+    nsec: 4294967295
+  liveliness: 2147483647
+  liveliness_lease_duration:
+    sec: 2147483647
+    nsec: 4294967295
+  avoid_ros_namespace_conventions: false

rosbag2_tests/test/rosbag2_tests/record_fixture.hpp

@@ -196,6 +196,15 @@ class RecordFixture : public TemporaryDirectoryFixture
     return std::get<0>(topic_format);
   }
 
+  std::string get_qos_profile_for_topic(
+    const std::string & topic_name, rosbag2_storage_plugins::SqliteWrapper & db)
+  {
+    std::string sql_query = "SELECT offered_qos_profiles FROM topics WHERE name = ?;";
+    auto qos_profile = db.prepare_statement(sql_query)
+      ->bind(topic_name)->execute_query<std::string>().get_single_line();
+    return std::get<0>(qos_profile);
+  }
+
   // relative path to the root of the bag file.
   rcpputils::fs::path root_bag_path_;
 

rosbag2_tests/test/rosbag2_tests/test_rosbag2_record_end_to_end.cpp

@@ -564,3 +564,77 @@ TEST_F(RecordFixture, record_end_to_end_test_with_cache) {
     get_messages_for_topic<test_msgs::msg::Strings>(topic_name);
   EXPECT_THAT(test_topic_messages, SizeIs(Ge(expected_test_messages)));
 }
+
+TEST_F(RecordFixture, record_end_to_end_test_with_qos_profile) {
+  auto topic = "/test_topic";
+  auto message = get_messages_strings()[0];
+  message->string_value = "test";
+  size_t expected_test_messages = 3;
+
+  auto qos_profile_path =
+    rcpputils::fs::path(_SRC_RESOURCES_DIR_PATH) / "qos_test" / "qos_profile.yaml";
+  std::stringstream command;
+  command << "ros2 bag record" <<
+    " --qos-profile " << qos_profile_path.string() <<
+    " --output " << root_bag_path_.string() << " " << topic;
+  auto process_handle = start_execution(command.str());
+  wait_for_db();
+
+  pub_man_.add_publisher(topic, message, expected_test_messages);
+
+  const auto database_path = get_bag_file_path(0).string();
+
+  rosbag2_storage_plugins::SqliteWrapper db{
+    database_path, rosbag2_storage::storage_interfaces::IOFlag::READ_ONLY};
+  pub_man_.run_publishers(
+    [this, &db](const std::string & topic_name) {
+      return count_stored_messages(db, topic_name);
+    });
+
+  stop_execution(process_handle);
+
+  // TODO(piraka9011): Remove when stop_execution properly SIGINT on Windows.
+  // This is required since stop_execution hard kills the process on Windows,
+  // which prevents the metadata from being written.
+#ifdef _WIN32
+  rosbag2_storage::BagMetadata metadata{};
+  metadata.version = 4;
+  metadata.storage_identifier = "sqlite3";
+  metadata.relative_file_paths = {get_bag_file_path(0).string()};
+  metadata.duration = std::chrono::nanoseconds(0);
+  metadata.starting_time =
+    std::chrono::time_point<std::chrono::high_resolution_clock>(std::chrono::nanoseconds(0));
+  metadata.message_count = 0;
+  rosbag2_storage::MetadataIo metadata_io;
+  metadata_io.write_metadata(root_bag_path_.string(), metadata);
+#endif
+
+  wait_for_metadata();
+  auto test_topic_messages =
+    get_messages_for_topic<test_msgs::msg::Strings>(topic);
+  EXPECT_THAT(test_topic_messages, SizeIs(Ge(expected_test_messages)));
+
+  for (const auto & message : test_topic_messages) {
+    EXPECT_EQ(message->string_value, "test");
+  }
+
+  // sec and nsec are LONG_MAX and ULONG_MAX respectively
+  std::string expected_serialized_profiles =
+    "- history: 3\n"
+    "  depth: 0\n"
+    "  reliability: 1\n"
+    "  durability: 2\n"
+    "  deadline:\n"
+    "    sec: 2147483647\n"
+    "    nsec: 4294967295\n"
+    "  lifespan:\n"
+    "    sec: 2147483647\n"
+    "    nsec: 4294967295\n"
+    "  liveliness: 1\n"
+    "  liveliness_lease_duration:\n"
+    "    sec: 2147483647\n"
+    "    nsec: 4294967295\n"
+    "  avoid_ros_namespace_conventions: false";
+
+  EXPECT_THAT(get_qos_profile_for_topic(topic, db), Eq(expected_serialized_profiles));
+}

rosbag2_transport/CMakeLists.txt

@@ -139,6 +139,7 @@ if(BUILD_TESTING)
       $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
       $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/src/rosbag2_transport>
       $<INSTALL_INTERFACE:include>)
+    target_link_libraries(test_rosbag2_node rosbag2_transport)
     ament_target_dependencies(test_rosbag2_node
       ament_index_cpp
       rclcpp

rosbag2_transport/include/rosbag2_transport/record_options.hpp

@@ -32,6 +32,7 @@ struct RecordOptions
   std::string node_prefix = "";
   std::string compression_mode = "";
   std::string compression_format = "";
+  std::string qos_profiles = "";
   bool include_hidden_topics = false;
 };
 

rosbag2_transport/src/rosbag2_transport/qos.cpp

@@ -14,6 +14,25 @@
 
 #include "qos.hpp"
 
+namespace rosbag2_transport
+{
+std::ostream & operator<<(std::ostream & stream, const Rosbag2QoS & qos)
+{
+  const auto profile = qos.get_rmw_qos_profile();
+  stream << "history: " << profile.history << std::endl <<
+    "depth: " << profile.depth << std::endl <<
+    "reliability: " << profile.reliability << std::endl <<
+    "durability: " << profile.durability << std::endl <<
+    "deadline: " << profile.deadline.sec << "." << profile.deadline.nsec << std::endl <<
+    "lifespan: " << profile.lifespan.sec << "." << profile.lifespan.nsec << std::endl <<
+    "liveliness: " << profile.liveliness << std::endl <<
+    "liveliness_lease_duration: " << profile.liveliness_lease_duration.sec << "." <<
+    profile.liveliness_lease_duration.nsec << std::endl <<
+    "avoid_ros_namespace_conventions: " << profile.avoid_ros_namespace_conventions << std::endl;
+  return stream;
+}
+}
+
 namespace YAML
 {
 Node convert<rmw_time_t>::encode(const rmw_time_t & time)

rosbag2_transport/src/rosbag2_transport/qos.hpp

@@ -40,6 +40,8 @@ class Rosbag2QoS : public rclcpp::QoS
   : rclcpp::QoS(rmw_qos_profile_default.depth) {}
   explicit Rosbag2QoS(const rclcpp::QoS & value)
   : rclcpp::QoS(value) {}
+
+  friend std::ostream & operator<<(std::ostream & stream, const Rosbag2QoS & qos);
 };
 }  // namespace rosbag2_transport
 

rosbag2_transport/src/rosbag2_transport/recorder.cpp

@@ -51,6 +51,7 @@ Recorder::Recorder(std::shared_ptr<rosbag2_cpp::Writer> writer, std::shared_ptr<
 
 void Recorder::record(const RecordOptions & record_options)
 {
+  qos_profile_overrides_ = YAML::Load(record_options.qos_profiles);
   if (record_options.rmw_serialization_format.empty()) {
     throw std::runtime_error("No serialization format specified!");
   }
@@ -121,6 +122,7 @@ Recorder::get_missing_topics(const std::unordered_map<std::string, std::string>
 
 namespace
 {
+
 std::string serialized_offered_qos_profiles_for_topic(
   std::shared_ptr<rosbag2_transport::Rosbag2Node> node,
   const std::string & topic_name)
@@ -155,7 +157,6 @@ void Recorder::subscribe_topic(const rosbag2_storage::TopicMetadata & topic)
   // that callback called before we reached out the line: writer_->create_topic(topic)
   writer_->create_topic(topic);
   auto subscription = create_subscription(topic.name, topic.type);
-
   if (subscription) {
     subscribed_topics_.insert(topic.name);
     subscriptions_.push_back(subscription);
@@ -170,10 +171,15 @@ std::shared_ptr<GenericSubscription>
 Recorder::create_subscription(
   const std::string & topic_name, const std::string & topic_type)
 {
+  auto subscription_qos = Rosbag2QoS(rclcpp::SystemDefaultsQoS());
+  if (qos_profile_overrides_[topic_name]) {
+    subscription_qos = qos_profile_overrides_[topic_name].as<Rosbag2QoS>();
+    ROSBAG2_TRANSPORT_LOG_INFO_STREAM("Overriding subscription profile for " << topic_name);
+  }
   auto subscription = node_->create_generic_subscription(
     topic_name,
     topic_type,
-    Rosbag2QoS{},
+    subscription_qos,
     [this, topic_name](std::shared_ptr<rmw_serialized_message_t> message) {
       auto bag_message = std::make_shared<rosbag2_storage::SerializedBagMessage>();
       bag_message->serialized_data = message;

rosbag2_transport/src/rosbag2_transport/recorder.hpp

@@ -23,6 +23,19 @@
 #include <utility>
 #include <vector>
 
+#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 "rosbag2_cpp/writer.hpp"
 
 #include "rosbag2_storage/topic_metadata.hpp"
@@ -76,6 +89,7 @@ class Recorder
   std::vector<std::shared_ptr<GenericSubscription>> subscriptions_;
   std::unordered_set<std::string> subscribed_topics_;
   std::string serialization_format_;
+  YAML::Node qos_profile_overrides_;
 };
 
 }  // namespace rosbag2_transport

rosbag2_transport/src/rosbag2_transport/rosbag2_transport_python.cpp

@@ -52,6 +52,7 @@ rosbag2_transport_record(PyObject * Py_UNUSED(self), PyObject * args, PyObject *
     "max_cache_size",
     "topics",
     "include_hidden_topics",
+    "qos_profiles",
     nullptr};
 
   char * uri = nullptr;
@@ -60,6 +61,7 @@ rosbag2_transport_record(PyObject * Py_UNUSED(self), PyObject * args, PyObject *
   char * node_prefix = nullptr;
   char * compression_mode = nullptr;
   char * compression_format = nullptr;
+  char * qos_profiles = nullptr;
   bool all = false;
   bool no_discovery = false;
   uint64_t polling_interval_ms = 100;
@@ -69,7 +71,7 @@ rosbag2_transport_record(PyObject * Py_UNUSED(self), PyObject * args, PyObject *
   bool include_hidden_topics = false;
   if (
     !PyArg_ParseTupleAndKeywords(
-      args, kwargs, "ssssss|bbKKKOb", const_cast<char **>(kwlist),
+      args, kwargs, "ssssss|bbKKKObs", const_cast<char **>(kwlist),
       &uri,
       &storage_id,
       &serilization_format,
@@ -82,8 +84,8 @@ rosbag2_transport_record(PyObject * Py_UNUSED(self), PyObject * args, PyObject *
       &max_bagfile_size,
       &max_cache_size,
       &topics,
-      &include_hidden_topics
-  ))
+      &include_hidden_topics,
+      &qos_profiles))
   {
     return nullptr;
   }
@@ -99,6 +101,7 @@ rosbag2_transport_record(PyObject * Py_UNUSED(self), PyObject * args, PyObject *
   record_options.compression_mode = std::string(compression_mode);
   record_options.compression_format = compression_format;
   record_options.include_hidden_topics = include_hidden_topics;
+  record_options.qos_profiles = qos_profiles;
 
   rosbag2_compression::CompressionOptions compression_options{
     record_options.compression_format,