Override QoS Profiles in CLI - Playback (ros2#356)

* Add overrides to play verb

Signed-off-by: Anas Abou Allaban <aabouallaban@pm.me>

ros2bag/ros2bag/verb/play.py

@@ -12,10 +12,16 @@
 # See the License for the specific language governing permissions and
 # limitations under the License.
 
+from argparse import FileType
+
+from rclpy.qos import InvalidQoSProfileException
 from ros2bag.api import check_path_exists
 from ros2bag.api import check_positive_float
+from ros2bag.api import convert_yaml_to_qos_profile
+from ros2bag.api import print_error
 from ros2bag.verb import VerbExtension
 from ros2cli.node import NODE_NAME_PREFIX
+import yaml
 
 
 class PlayVerb(VerbExtension):
@@ -35,8 +41,20 @@ def add_arguments(self, parser, cli_name):  # noqa: D102
         parser.add_argument(
             '-r', '--rate', type=check_positive_float, default=1.0,
             help='rate at which to play back messages. Valid range > 0.0.')
+        parser.add_argument(
+            '--qos-profile-overrides-path', type=FileType('r'),
+            help='Path to a yaml file defining overrides of the QoS profile for specific topics.')
 
     def main(self, *, args):  # noqa: D102
+        qos_profile_overrides = {}  # Specify a valid default
+        if args.qos_profile_overrides_path:
+            qos_profile_dict = yaml.safe_load(args.qos_profile_overrides_path)
+            try:
+                qos_profile_overrides = convert_yaml_to_qos_profile(
+                    qos_profile_dict)
+            except (InvalidQoSProfileException, ValueError) as e:
+                return print_error(str(e))
+
         # NOTE(hidmic): in merged install workspaces on Windows, Python entrypoint lookups
         #               combined with constrained environments (as imposed by colcon test)
         #               may result in DLL loading failures when attempting to import a C
@@ -48,4 +66,5 @@ def main(self, *, args):  # noqa: D102
             storage_id=args.storage,
             node_prefix=NODE_NAME_PREFIX,
             read_ahead_queue_size=args.read_ahead_queue_size,
-            rate=args.rate)
+            rate=args.rate,
+            qos_profile_overrides=qos_profile_overrides)

ros2bag/test/resources/empty_bag/metadata.yaml

@@ -0,0 +1,25 @@
+rosbag2_bagfile_information:
+  version: 4
+  storage_identifier: sqlite3
+  relative_file_paths:
+    - empty_bag_0.db3
+  duration:
+    nanoseconds: 0
+  starting_time:
+    nanoseconds_since_epoch: 9223372036854775807
+  message_count: 0
+  topics_with_message_count:
+    - topic_metadata:
+        name: /parameter_events
+        type: rcl_interfaces/msg/ParameterEvent
+        serialization_format: cdr
+        offered_qos_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"
+      message_count: 0
+    - topic_metadata:
+        name: /rosout
+        type: rcl_interfaces/msg/Log
+        serialization_format: cdr
+        offered_qos_profiles: "- history: 3\n  depth: 0\n  reliability: 1\n  durability: 1\n  deadline:\n    sec: 2147483647\n    nsec: 4294967295\n  lifespan:\n    sec: 10\n    nsec: 0\n  liveliness: 1\n  liveliness_lease_duration:\n    sec: 2147483647\n    nsec: 4294967295\n  avoid_ros_namespace_conventions: false"
+      message_count: 0
+  compression_format: ""
+  compression_mode: ""

ros2bag/test/test_play_qos_profiles.py

@@ -0,0 +1,84 @@
+# 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.
+
+import contextlib
+from pathlib import Path
+import re
+import unittest
+
+from launch import LaunchDescription
+from launch.actions import ExecuteProcess
+import launch_testing
+import launch_testing.actions
+import launch_testing.asserts
+import launch_testing.markers
+import launch_testing.tools
+
+import pytest
+
+
+RESOURCES_PATH = Path(__file__).parent / 'resources'
+TEST_NODE = 'ros2bag_record_qos_profile_test_node'
+TEST_NAMESPACE = 'ros2bag_record_qos_profile'
+ERROR_STRING = r'\[ERROR] \[ros2bag]:'
+
+
+@pytest.mark.rostest
+@launch_testing.markers.keep_alive
+def generate_test_description():
+    return LaunchDescription([launch_testing.actions.ReadyToTest()])
+
+
+class TestRos2BagPlay(unittest.TestCase):
+
+    @classmethod
+    def setUpClass(cls, launch_service, proc_info, proc_output):
+        @contextlib.contextmanager
+        def launch_bag_command(self, arguments, **kwargs):
+            pkg_command_action = ExecuteProcess(
+                cmd=['ros2', 'bag', *arguments],
+                additional_env={'PYTHONUNBUFFERED': '1'},
+                name='ros2bag-cli',
+                output='screen',
+                **kwargs
+            )
+            with launch_testing.tools.launch_process(
+                    launch_service, pkg_command_action, proc_info, proc_output
+            ) as pkg_command:
+                yield pkg_command
+        cls.launch_bag_command = launch_bag_command
+
+    def test_qos_simple(self):
+        """Test with a full QoS profile override for a single topic."""
+        profile_path = RESOURCES_PATH / 'qos_profile.yaml'
+        bag_path = RESOURCES_PATH / 'empty_bag'
+        arguments = ['play', '--qos-profile-overrides-path', profile_path.as_posix(),
+                     bag_path.as_posix()]
+        with self.launch_bag_command(arguments=arguments) as bag_command:
+            bag_command.wait_for_shutdown(timeout=5)
+        expected_string_regex = re.compile(ERROR_STRING)
+        matches = expected_string_regex.search(bag_command.output)
+        assert not matches, print('ros2bag CLI did not produce the expected output')
+
+    def test_qos_incomplete(self):
+        """Test a partially filled QoS profile for a single topic."""
+        profile_path = RESOURCES_PATH / 'incomplete_qos_profile.yaml'
+        bag_path = RESOURCES_PATH / 'empty_bag'
+        arguments = ['play', '--qos-profile-overrides-path', profile_path.as_posix(),
+                     bag_path.as_posix()]
+        with self.launch_bag_command(arguments=arguments) as bag_command:
+            bag_command.wait_for_shutdown(timeout=5)
+        expected_string_regex = re.compile(ERROR_STRING)
+        matches = expected_string_regex.search(bag_command.output)
+        assert not matches, print('ros2bag CLI did not produce the expected output')

rosbag2_transport/src/rosbag2_transport/rosbag2_transport_python.cpp

@@ -56,6 +56,27 @@ rmw_qos_profile_t * PyQoSProfile_AsRmwQoSProfile(PyObject * object)
   return reinterpret_cast<rmw_qos_profile_t *>(
     PyCapsule_GetPointer(py_capsule, "rmw_qos_profile_t"));
 }
+
+std::unordered_map<std::string, rclcpp::QoS> PyObject_AsTopicQoSMap(PyObject * object)
+{
+  std::unordered_map<std::string, rclcpp::QoS> topic_qos_overrides{};
+  if (PyDict_Check(object)) {
+    PyObject * key{nullptr};
+    PyObject * value{nullptr};
+    Py_ssize_t pos{0};
+    while (PyDict_Next(object, &pos, &key, &value)) {
+      auto topic_name = PyObject_AsStdString(key);
+      auto rmw_qos_profile = PyQoSProfile_AsRmwQoSProfile(value);
+      auto qos_init = rclcpp::QoSInitialization::from_rmw(*rmw_qos_profile);
+      auto qos_profile = rclcpp::QoS(qos_init, *rmw_qos_profile);
+      topic_qos_overrides.insert({topic_name, qos_profile});
+    }
+  } else {
+    throw std::runtime_error{"QoS profile overrides object is not a Python dictionary."};
+  }
+  return topic_qos_overrides;
+}
+
 }  // namespace
 
 static PyObject *
@@ -134,21 +155,7 @@ rosbag2_transport_record(PyObject * Py_UNUSED(self), PyObject * args, PyObject *
     rosbag2_compression::compression_mode_from_string(record_options.compression_mode)
   };
 
-  if (PyDict_Check(qos_profile_overrides)) {
-    PyObject * key = nullptr;
-    PyObject * value = nullptr;
-    Py_ssize_t pos = 0;
-    std::unordered_map<std::string, rclcpp::QoS> topic_qos_overrides{};
-    while (PyDict_Next(qos_profile_overrides, &pos, &key, &value)) {
-      auto topic_name = PyObject_AsStdString(key);
-      auto rmw_qos_profile = PyQoSProfile_AsRmwQoSProfile(value);
-      auto qos_init = rclcpp::QoSInitialization::from_rmw(*rmw_qos_profile);
-      auto qos_profile = rclcpp::QoS(qos_init, *rmw_qos_profile);
-      topic_qos_overrides.insert(std::pair<std::string, rclcpp::QoS>(topic_name, qos_profile));
-    }
-  } else {
-    throw std::runtime_error{"QoS profile overrides object is not a Python dictionary."};
-  }
+  auto topic_qos_overrides = PyObject_AsTopicQoSMap(qos_profile_overrides);
 
   if (topics) {
     PyObject * topic_iterator = PyObject_GetIter(topics);
@@ -200,6 +207,7 @@ rosbag2_transport_play(PyObject * Py_UNUSED(self), PyObject * args, PyObject * k
     "node_prefix",
     "read_ahead_queue_size",
     "rate",
+    "qos_profile_overrides",
     nullptr
   };
 
@@ -208,13 +216,15 @@ rosbag2_transport_play(PyObject * Py_UNUSED(self), PyObject * args, PyObject * k
   char * node_prefix;
   size_t read_ahead_queue_size;
   float rate;
+  PyObject * qos_profile_overrides{nullptr};
   if (!PyArg_ParseTupleAndKeywords(
-      args, kwargs, "sss|kf", const_cast<char **>(kwlist),
+      args, kwargs, "sss|kfO", const_cast<char **>(kwlist),
       &uri,
       &storage_id,
       &node_prefix,
       &read_ahead_queue_size,
-      &rate))
+      &rate,
+      &qos_profile_overrides))
   {
     return nullptr;
   }
@@ -226,6 +236,8 @@ rosbag2_transport_play(PyObject * Py_UNUSED(self), PyObject * args, PyObject * k
   play_options.read_ahead_queue_size = read_ahead_queue_size;
   play_options.rate = rate;
 
+  auto topic_qos_overrides = PyObject_AsTopicQoSMap(qos_profile_overrides);
+
   rosbag2_storage::MetadataIo metadata_io{};
   rosbag2_storage::BagMetadata metadata{};
   // Specify defaults