rename record_bag to bag_record, add bag_play (#188)

docs/libraries.rst

@@ -894,6 +894,76 @@ Check the latency of the specified topic (in system time). If the check with ``c
      - Class of message type, only required when 'wait_for_first_message' is set to false (e.g. ``std_msgs.msg.String``)
 
 
+``bag_play()``
+^^^^^^^^^^^^^^^
+
+Play back a ROS bag.
+
+.. list-table:: 
+   :widths: 15 15 5 65
+   :header-rows: 1
+   :class: tight-table   
+
+   * - Parameter
+     - Type
+     - Default
+     - Description
+   * - ``source``
+     - ``string``
+     - 
+     - path to ROS bag directory, either absolute or relative to scenario-file directory
+   * - ``topics``
+     - ``list of string``
+     - 
+     - topics to publish, if empty all topics are published
+   * - ``publish_clock``
+     - ``bool``
+     - ``false``
+     - wether to publish to /clock
+   * - ``publish_clock_rate``
+     - ``float``
+     - ``1.0``
+     - if ``publish_clock`` is true, publish to ``/clock`` at the specified frequency in Hz, to act as a ROS Time Source.
+
+
+``bag_record()``
+^^^^^^^^^^^^^^^^
+
+Record a ROS bag, stored in directory ``output_dir`` defined by command-line parameter (default: ``.``). If ``topics`` is specified, this action waits for all topics to be subscribed until it returns with success otherwise it immediately returns. The recording is active until the end of the scenario.
+
+A common topic to record is ``/scenario_execution/snapshots`` which publishes changes within the behavior tree. When replaying the bag-file, this allows to visualize the current state of the scenario in RViz, using the ``scenario_execution_rviz`` plugin.
+
+.. list-table:: 
+   :widths: 15 15 5 65
+   :header-rows: 1
+   :class: tight-table   
+
+   * - Parameter
+     - Type
+     - Default
+     - Description
+   * - ``topics``
+     - ``list of string``
+     - 
+     - List of topics to capture
+   * - ``timestamp_suffix``
+     - ``bool``
+     - ``true``
+     - Add a timestamp suffix to output directory name
+   * - ``hidden_topics``
+     - ``bool``
+     - ``false``
+     - Whether to record hidden topics
+   * - ``storage``
+     - ``string``
+     - ``''``
+     - Storage type to use (empty string: use ROS bag record default)
+   * - ``use_sim_time``
+     - ``bool``
+     - ``false``
+     - Use simulation time for message timestamps by subscribing to the /clock topic
+
+
 ``check_data()``
 ^^^^^^^^^^^^^^^^
 
@@ -1026,43 +1096,6 @@ Wait for specific output in ROS log (i.e. ``/rosout`` topic). If any of the entr
      - 
      - list of strings (in python syntax, e.g. "[\'foo\', \'bar\']")
 
-``record_bag()``
-^^^^^^^^^^^^^^^^
-
-Record a ROS bag, stored in directory ``output_dir`` defined by command-line parameter (default: '.').
-
-A common topic to record is ``/scenario_execution/snapshots`` which publishes changes within the behavior tree. When replaying the bag-file, this allows to visualize the current state of the scenario in RViz, using the ``scenario_execution_rviz`` plugin.
-
-.. list-table:: 
-   :widths: 15 15 5 65
-   :header-rows: 1
-   :class: tight-table   
-
-   * - Parameter
-     - Type
-     - Default
-     - Description
-   * - ``topics``
-     - ``list of string``
-     - 
-     - List of topics to capture
-   * - ``timestamp_suffix``
-     - ``bool``
-     - ``true``
-     - Add a timestamp suffix to output directory name
-   * - ``hidden_topics``
-     - ``bool``
-     - ``false``
-     - Whether to record hidden topics
-   * - ``storage``
-     - ``string``
-     - ``''``
-     - Storage type to use (empty string: use ROS bag record default)
-   * - ``use_sim_time``
-     - ``bool``
-     - ``false``
-     - Use simulation time for message timestamps by subscribing to the /clock topic
-
 ``ros_launch()``
 ^^^^^^^^^^^^^^^^
 

examples/example_multi_robot/scenarios/example_multi_robot.osc

@@ -11,7 +11,7 @@ scenario multi_robot:
         robot2.spawn(pose_3d(position_3d(x: -3.0, y: 1.5, z: 0.3), orientation_3d(yaw: -1.57)), model: 'example_multi_robot://models/robot.sdf')
         ros_launch("example_multi_robot", "robot2_launch.py", wait_for_shutdown: false)
         ros_launch("gazebo_static_camera", "spawn_static_camera_launch.py", [ key_value('x', '-3'), key_value('z', '10'), key_value('pitch', '1.57')], wait_for_shutdown: false)
-        record_bag(['/tf', '/tf_static', '/scenario_execution/snapshots', '/map', '/static_camera/image_raw', '/local_costmap/costmap'], use_sim_time: true)
+        bag_record(['/tf', '/tf_static', '/scenario_execution/snapshots', '/map', '/static_camera/image_raw', '/local_costmap/costmap'], use_sim_time: true)
         parallel:
             test_drive: serial:
                 robot.init_nav2(initial_pose: pose_3d())

scenario_coverage/scenarios/test_fault_injection.osc

@@ -7,7 +7,7 @@ scenario nav2_simulation_fault_injection:
     robot: differential_drive_robot
     do serial:
         robot.init_nav2(pose_3d(position_3d(x: 0.0m, y: 0.0m)))
-        record_bag(topics: ['/robot_pose_loc', '/robot_pose_gt', '/cmd_vel', '/amcl_pose', '/odom', '/tf', '/tf_static'], timestamp_suffix: false)
+        bag_record(topics: ['/robot_pose_loc', '/robot_pose_gt', '/cmd_vel', '/amcl_pose', '/odom', '/tf', '/tf_static'], timestamp_suffix: false)
         noise: set_node_parameter(node_name: 'laserscan_modification',
                             parameter_name: 'gaussian_noise_std_deviation') with:
             keep(it.parameter_value in ['0.0', '0.1', '0.2', '0.3', '0.4', '0.5', '0.6', '0.7'])

scenario_execution/scenario_execution/actions/run_process.py

@@ -62,6 +62,7 @@ def update(self) -> py_trees.common.Status:
                 return py_trees.common.Status.FAILURE
 
             self.feedback_message = f"Executing '{self.command}'"  # pylint: disable= attribute-defined-outside-init
+            self.logger.debug(f"Executing '{self.command}'")
             self.on_executed()
 
             def log_output(out, log_fct, buffer):

scenario_execution_ros/scenario_execution_ros/actions/ros_bag_play.py

@@ -0,0 +1,69 @@
+# Copyright (C) 2024 Intel Corporation
+#
+# 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.
+#
+# SPDX-License-Identifier: Apache-2.0
+
+import os
+import py_trees
+from scenario_execution.actions.base_action import ActionError
+from scenario_execution.actions.run_process import RunProcess
+
+
+class RosBagPlay(RunProcess):
+    """
+    Class to execute ros bag recording
+    """
+
+    def __init__(self):
+        super().__init__()
+        self.input_dir = None
+        self.source = None
+
+    def setup(self, **kwargs):
+        if "input_dir" not in kwargs:
+            raise ActionError("input_dir not defined.", action=self)
+        self.input_dir = kwargs['input_dir']
+
+    def execute(self, source: str, topics: list, publish_clock: bool, publish_clock_rate: float):  # pylint: disable=arguments-differ,arguments-renamed
+        super().execute(wait_for_shutdown=True)
+        self.source = source
+        bag_dir = ''
+        if os.path.isabs(source):
+            bag_dir = source
+        else:
+            bag_dir = os.path.join(self.input_dir, source)
+        if not os.path.exists(bag_dir):
+            raise ActionError(f"Specified rosbag directory '{bag_dir}' does not exist", action=self)
+
+        self.command = ["ros2", "bag", "play"]
+        if publish_clock:
+            self.command.extend(["--clock", str(publish_clock_rate)])
+        if topics:
+            topics_string = " ".join(topics)
+            self.command.append(f"--topics '{topics_string}'")
+        self.command.append(bag_dir)
+
+    def get_logger_stderr(self):
+        return self.logger.info  # ros2 bag play reports all messages on stderr
+
+    def on_executed(self):
+        self.feedback_message = f"Playing back {self.source}..."  # pylint: disable= attribute-defined-outside-init
+
+    def on_process_finished(self, ret):
+        if ret == 0:
+            self.feedback_message = f"Playback of {self.source} finished."  # pylint: disable= attribute-defined-outside-init
+            return py_trees.common.Status.SUCCESS
+        else:
+            self.feedback_message = f"Playback of '{self.source}' failed with {ret}"  # pylint: disable= attribute-defined-outside-init
+            return py_trees.common.Status.FAILURE

scenario_execution_ros/scenario_execution_ros/lib_osc/ros.osc

@@ -70,6 +70,21 @@ action assert_topic_latency:
     wait_for_first_message: bool = true  # start measuring with the first received message
     topic_type: string                   # class of message type, only required when wait_for_first_message is set to false (e.g. std_msgs.msg.String)
 
+action bag_play:
+    # play back a ros bag
+    source: string                       # path to rosbag directory, either absolute or relative to scenario-file directory
+    topics: list of string               # topics to publish, if empty all topics are published
+    publish_clock: bool = false          # wether to publish to /clock
+    publish_clock_rate: float = 1.0      # if publish_clock is true, publish to /clock at the specified frequency in Hz, to act as a ROS Time Source.
+
+action bag_record:
+    # Record a ros bag, stored in output_dir defined by command-line parameter (default: '.')
+    topics: list of string               # topics to records, if empty all topics are recorded
+    timestamp_suffix: bool = true        # add a timestamp suffix to output directory name
+    hidden_topics: bool = false          # whether to record hidden topics
+    storage: string = ''                 # storage type to use (empty string: use default)
+    use_sim_time: bool = false           # use simulation time for message timestamps by subscribing to the /clock topic
+
 action check_data:
     # Compare received topic messages using the given comparison_operator, against the specified value. Either the whole message gets compared or a member defined by member_name.
     topic_name: string                   # name of the topic to connect to
@@ -100,14 +115,6 @@ action log_check:
     module_name: string = ''             # If specified, a matching message must also match the module name
     values: list of string               # string to check for. If found, action succeeds
 
-action record_bag:
-    # Record a dataset, stored in output_dir defined by command-line parameter (default: '.')
-    topics: list of topics               # Topics to records, if empty all topics are recorded
-    timestamp_suffix: bool = true        # Add a timestamp suffix to output directory name
-    hidden_topics: bool = false          # whether to record hidden topics
-    storage: string = ''                 # storage type to use (empty string: use default)
-    use_sim_time: bool = false           # use simulation time for message timestamps by subscribing to the /clock topic
-
 action ros_launch:
     # Execute a ros launch file
     package_name: string                 # package that contains the launch file

scenario_execution_ros/scenario_execution_ros/logging_ros.py

@@ -30,6 +30,8 @@ class RosLogger(BaseLogger):
 
     def __init__(self, name: str, debug=False):
         super().__init__(name, debug)
+        if debug:
+            rclpy.logging.set_logger_level(name, rclpy.logging.LoggingSeverity.DEBUG)
         self.logger = rclpy.logging.get_logger(name)
 
     def info(self, msg: str):

scenario_execution_ros/setup.py

@@ -53,11 +53,12 @@
             'assert_topic_latency = scenario_execution_ros.actions.assert_topic_latency:AssertTopicLatency',
             'assert_tf_moving = scenario_execution_ros.actions.assert_tf_moving:AssertTfMoving',
             'assert_lifecycle_state = scenario_execution_ros.actions.assert_lifecycle_state:AssertLifecycleState',
+            'bag_play = scenario_execution_ros.actions.ros_bag_play:RosBagPlay',
+            'bag_record = scenario_execution_ros.actions.ros_bag_record:RosBagRecord',
             'check_data = scenario_execution_ros.actions.ros_topic_check_data:RosTopicCheckData',
             'differential_drive_robot.odometry_distance_traveled = scenario_execution_ros.actions.odometry_distance_traveled:OdometryDistanceTraveled',
             'differential_drive_robot.tf_close_to = scenario_execution_ros.actions.tf_close_to:TfCloseTo',
             'log_check = scenario_execution_ros.actions.ros_log_check:RosLogCheck',
-            'record_bag = scenario_execution_ros.actions.ros_bag_record:RosBagRecord',
             'ros_launch = scenario_execution_ros.actions.ros_launch:RosLaunch',
             'service_call = scenario_execution_ros.actions.ros_service_call:RosServiceCall',
             'set_node_parameter = scenario_execution_ros.actions.ros_set_node_parameter:RosSetNodeParameter',

test/scenario_execution_nav2_test/scenarios/test_scenario_execution_nav2.osc

@@ -7,7 +7,7 @@ scenario example_nav2:
     robot: differential_drive_robot
     do serial:
         ros_launch("gazebo_static_camera", "spawn_static_camera_launch.py", [ key_value('z', '10'), key_value('pitch', '1.57')], wait_for_shutdown: false)
-        record_bag(['/tf', '/tf_static', '/scenario_execution/snapshots', '/map', '/static_camera/image_raw', '/local_costmap/costmap'], use_sim_time: true)
+        bag_record(['/tf', '/tf_static', '/scenario_execution/snapshots', '/map', '/static_camera/image_raw', '/local_costmap/costmap'], use_sim_time: true)
         robot.init_nav2(pose_3d(position_3d(x: 0.0m, y: 0.0m)))
         serial:
             repeat(2)