update record_bag action

.github/workflows/test_build.yml

@@ -309,6 +309,12 @@ jobs:
         with:
           name: test-example-multirobot-result
           path: test_example_multirobot/test.xml
+      - name: Upload ros bag
+        uses: actions/upload-artifact@ef09cdac3e2d3e60d8ccadda691f4f1cec5035cb
+        if: always()
+        with:
+          name: test-example-multirobot-ros-bag
+          path: test_example_multirobot/**/rosbag*
   test-example-external-method:
     needs: [build]
     runs-on: intellabs-01

docs/libraries.rst

@@ -725,8 +725,12 @@ A common topic to record is ``/scenario_execution/snapshots`` which publishes ch
      - 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/README.md

@@ -13,24 +13,10 @@ source install/setup.bash
 ```
 The Following example spawns 2 robots. The first robot is initialised with nav2 and moves in a straight line. Once it reaches a specific point a velocity command is sent to the second robot which will position itsel in front of the first one. The first robot then replans its path and moves around the second robot.
 
-To actually run the scenario, run the following commands.
-Note: due to an [issue](https://github.com/turtlebot/turtlebot4_simulator/issues/60) with the turtlebot simulation when running multiple robots, some launch commands need to run in separate terminals.
-Launch the simulation and spawn the first robot by running the following command:
+To actually run the scenario, run the following command.
 
 ```bash
-ros2 launch tb4_sim_scenario sim_nav_scenario_launch.py scenario_execution:=False scenario:=foo yaw:=3.14
-```
-
-To spawn the second robot, run the following command in a new terminal:
-
-```bash
-ros2 launch tb4_sim_scenario ignition_robot_launch.py namespace:=turtlebot2 x:=-3.0 y:=1.5 yaw:=-1.57
-```
-
-To run the actual scenario, run the following command in a third terminal:
-
-```bash
-ros2 launch scenario_execution scenario_launch.py scenario:=examples/example_multi_robot/example_multi_robot.osc
+ros2 launch tb4_sim_scenario sim_nav_scenario_launch.py scenario:=examples/example_multi_robot/example_multi_robot.osc
 ```
 
 For a more detailed understanding of the code structure and scenario implementation please refer to the [tutorial documentation](https://intellabs.github.io/scenario_execution/tutorials.html).

examples/example_multi_robot/example_multi_robot.osc

@@ -1,179 +1,31 @@
 import osc.ros
+import osc.gazebo
 
-scenario nav2_simulation_nav_to_pose:
+scenario multi_robot:
     robot: differential_drive_robot
+    robot2: osc_actor
     do parallel:
         serial:
-            parallel:   
+            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', '/initialpose'], use_sim_time: true)
+            parallel:
                 test_drive: serial:
-                    robot.init_nav2(initial_pose: pose_3d(position: position_3d(), orientation: orientation_3d(yaw: 3.14rad)))
+                    robot.init_nav2(initial_pose: pose_3d(orientation: orientation_3d(yaw: 3.14rad)))
                     robot.nav_to_pose(goal_pose: pose_3d(position: position_3d(x: -4.0m), orientation: orientation_3d(yaw: 3.14rad)))
+                    emit end
                 serial:
                     robot.tf_close_to() with:
                         keep(it.reference_point.x == -1.5m)
                         keep(it.reference_point.y == 0.0m)
                         keep(it.threshold == 0.35m)
                         keep(it.robot_frame_id ==  'turtlebot4_base_link_gt')
                     topic_publish() with:
-                        keep(it.topic_name == '/turtlebot2/cmd_vel')
+                        keep(it.topic_name == '/robot2/cmd_vel')
                         keep(it.topic_type == 'geometry_msgs.msg.Twist')
-                        keep(it.value == '{\"linear\": {\"x\": 0.3, \"y\": 0.0, \"z\": 0.0}, \"angular\": {\"x\": 0.0, \"y\": 0.0, \"z\": 0.0}}')
-                    wait elapsed(0.2s)
-                    topic_publish() with:
-                        keep(it.topic_name == '/turtlebot2/cmd_vel')     
-                        keep(it.topic_type == 'geometry_msgs.msg.Twist')    
-                        keep(it.value == '{\"linear\": {\"x\": 0.30, \"y\": 0.0, \"z\": 0.0}, \"angular\": {\"x\": 0.0, \"y\": 0.0, \"z\": 0.0}}') 
-                    wait elapsed(0.2s)
-                    topic_publish() with:
-                        keep(it.topic_name == '/turtlebot2/cmd_vel')     
-                        keep(it.topic_type == 'geometry_msgs.msg.Twist')    
-                        keep(it.value == '{\"linear\": {\"x\": 0.30, \"y\": 0.0, \"z\": 0.0}, \"angular\": {\"x\": 0.0, \"y\": 0.0, \"z\": 0.0}}') 
-                    wait elapsed(0.2s)
-                    topic_publish() with:
-                        keep(it.topic_name == '/turtlebot2/cmd_vel')     
-                        keep(it.topic_type == 'geometry_msgs.msg.Twist')    
-                        keep(it.value == '{\"linear\": {\"x\": 0.30, \"y\": 0.0, \"z\": 0.0}, \"angular\": {\"x\": 0.0, \"y\": 0.0, \"z\": 0.0}}') 
-                    wait elapsed(0.2s)
-                    topic_publish() with:
-                        keep(it.topic_name == '/turtlebot2/cmd_vel')     
-                        keep(it.topic_type == 'geometry_msgs.msg.Twist')    
-                        keep(it.value == '{\"linear\": {\"x\": 0.30, \"y\": 0.0, \"z\": 0.0}, \"angular\": {\"x\": 0.0, \"y\": 0.0, \"z\": 0.0}}') 
-                    wait elapsed(0.2s)
-                    topic_publish() with:
-                        keep(it.topic_name == '/turtlebot2/cmd_vel')     
-                        keep(it.topic_type == 'geometry_msgs.msg.Twist')    
-                        keep(it.value == '{\"linear\": {\"x\": 0.30, \"y\": 0.0, \"z\": 0.0}, \"angular\": {\"x\": 0.0, \"y\": 0.0, \"z\": 0.0}}') 
-                    wait elapsed(0.2s)
-                    topic_publish() with:
-                        keep(it.topic_name == '/turtlebot2/cmd_vel')     
-                        keep(it.topic_type == 'geometry_msgs.msg.Twist')    
-                        keep(it.value == '{\"linear\": {\"x\": 0.30, \"y\": 0.0, \"z\": 0.0}, \"angular\": {\"x\": 0.0, \"y\": 0.0, \"z\": 0.0}}') 
-                    wait elapsed(0.2s)
-                    topic_publish() with:
-                        keep(it.topic_name == '/turtlebot2/cmd_vel')     
-                        keep(it.topic_type == 'geometry_msgs.msg.Twist')    
-                        keep(it.value == '{\"linear\": {\"x\": 0.30, \"y\": 0.0, \"z\": 0.0}, \"angular\": {\"x\": 0.0, \"y\": 0.0, \"z\": 0.0}}') 
-                    wait elapsed(0.2s)
-                    topic_publish() with:
-                        keep(it.topic_name == '/turtlebot2/cmd_vel')     
-                        keep(it.topic_type == 'geometry_msgs.msg.Twist')    
-                        keep(it.value == '{\"linear\": {\"x\": 0.30, \"y\": 0.0, \"z\": 0.0}, \"angular\": {\"x\": 0.0, \"y\": 0.0, \"z\": 0.0}}') 
-                    wait elapsed(0.2s)
-                    topic_publish() with:
-                        keep(it.topic_name == '/turtlebot2/cmd_vel')     
-                        keep(it.topic_type == 'geometry_msgs.msg.Twist')    
-                        keep(it.value == '{\"linear\": {\"x\": 0.30, \"y\": 0.0, \"z\": 0.0}, \"angular\": {\"x\": 0.0, \"y\": 0.0, \"z\": 0.0}}') 
-                    wait elapsed(0.2s)
-                    topic_publish() with:
-                        keep(it.topic_name == '/turtlebot2/cmd_vel')     
-                        keep(it.topic_type == 'geometry_msgs.msg.Twist')    
-                        keep(it.value == '{\"linear\": {\"x\": 0.30, \"y\": 0.0, \"z\": 0.0}, \"angular\": {\"x\": 0.0, \"y\": 0.0, \"z\": 0.0}}') 
-                    wait elapsed(0.2s)
-                    topic_publish() with:
-                        keep(it.topic_name == '/turtlebot2/cmd_vel')     
-                        keep(it.topic_type == 'geometry_msgs.msg.Twist')    
-                        keep(it.value == '{\"linear\": {\"x\": 0.30, \"y\": 0.0, \"z\": 0.0}, \"angular\": {\"x\": 0.0, \"y\": 0.0, \"z\": 0.0}}') 
-                    wait elapsed(0.2s)
-                    topic_publish() with:
-                        keep(it.topic_name == '/turtlebot2/cmd_vel')     
-                        keep(it.topic_type == 'geometry_msgs.msg.Twist')    
-                        keep(it.value == '{\"linear\": {\"x\": 0.30, \"y\": 0.0, \"z\": 0.0}, \"angular\": {\"x\": 0.0, \"y\": 0.0, \"z\": 0.0}}') 
-                    wait elapsed(0.2s)
-                    topic_publish() with:
-                        keep(it.topic_name == '/turtlebot2/cmd_vel')     
-                        keep(it.topic_type == 'geometry_msgs.msg.Twist')    
-                        keep(it.value == '{\"linear\": {\"x\": 0.30, \"y\": 0.0, \"z\": 0.0}, \"angular\": {\"x\": 0.0, \"y\": 0.0, \"z\": 0.0}}') 
-                    wait elapsed(0.2s)
-                    topic_publish() with:
-                        keep(it.topic_name == '/turtlebot2/cmd_vel')     
-                        keep(it.topic_type == 'geometry_msgs.msg.Twist')    
-                        keep(it.value == '{\"linear\": {\"x\": 0.30, \"y\": 0.0, \"z\": 0.0}, \"angular\": {\"x\": 0.0, \"y\": 0.0, \"z\": 0.0}}') 
-                    wait elapsed(0.2s)
-                    topic_publish() with:
-                        keep(it.topic_name == '/turtlebot2/cmd_vel')     
-                        keep(it.topic_type == 'geometry_msgs.msg.Twist')    
-                        keep(it.value == '{\"linear\": {\"x\": 0.30, \"y\": 0.0, \"z\": 0.0}, \"angular\": {\"x\": 0.0, \"y\": 0.0, \"z\": 0.0}}') 
-                    wait elapsed(0.2s)
-                    topic_publish() with:
-                        keep(it.topic_name == '/turtlebot2/cmd_vel')     
-                        keep(it.topic_type == 'geometry_msgs.msg.Twist')    
-                        keep(it.value == '{\"linear\": {\"x\": 0.30, \"y\": 0.0, \"z\": 0.0}, \"angular\": {\"x\": 0.0, \"y\": 0.0, \"z\": 0.0}}') 
-                    wait elapsed(0.2s)
-                    topic_publish() with:
-                        keep(it.topic_name == '/turtlebot2/cmd_vel')     
-                        keep(it.topic_type == 'geometry_msgs.msg.Twist')    
-                        keep(it.value == '{\"linear\": {\"x\": 0.30, \"y\": 0.0, \"z\": 0.0}, \"angular\": {\"x\": 0.0, \"y\": 0.0, \"z\": 0.0}}') 
-                    wait elapsed(0.2s)
-                    topic_publish() with:
-                        keep(it.topic_name == '/turtlebot2/cmd_vel')     
-                        keep(it.topic_type == 'geometry_msgs.msg.Twist')    
-                        keep(it.value == '{\"linear\": {\"x\": 0.30, \"y\": 0.0, \"z\": 0.0}, \"angular\": {\"x\": 0.0, \"y\": 0.0, \"z\": 0.0}}') 
-                    wait elapsed(0.2s)
-                    topic_publish() with:
-                        keep(it.topic_name == '/turtlebot2/cmd_vel')     
-                        keep(it.topic_type == 'geometry_msgs.msg.Twist')    
-                        keep(it.value == '{\"linear\": {\"x\": 0.30, \"y\": 0.0, \"z\": 0.0}, \"angular\": {\"x\": 0.0, \"y\": 0.0, \"z\": 0.0}}') 
-                    wait elapsed(0.2s)
-                    topic_publish() with:
-                        keep(it.topic_name == '/turtlebot2/cmd_vel')     
-                        keep(it.topic_type == 'geometry_msgs.msg.Twist')    
-                        keep(it.value == '{\"linear\": {\"x\": 0.30, \"y\": 0.0, \"z\": 0.0}, \"angular\": {\"x\": 0.0, \"y\": 0.0, \"z\": 0.0}}') 
-                    wait elapsed(0.2s)
-                    topic_publish() with:
-                        keep(it.topic_name == '/turtlebot2/cmd_vel')     
-                        keep(it.topic_type == 'geometry_msgs.msg.Twist')    
-                        keep(it.value == '{\"linear\": {\"x\": 0.30, \"y\": 0.0, \"z\": 0.0}, \"angular\": {\"x\": 0.0, \"y\": 0.0, \"z\": 0.0}}') 
-                    wait elapsed(0.2s)
-                    topic_publish() with:
-                        keep(it.topic_name == '/turtlebot2/cmd_vel')     
-                        keep(it.topic_type == 'geometry_msgs.msg.Twist')    
-                        keep(it.value == '{\"linear\": {\"x\": 0.30, \"y\": 0.0, \"z\": 0.0}, \"angular\": {\"x\": 0.0, \"y\": 0.0, \"z\": 0.0}}') 
-                    wait elapsed(0.2s)
-                    topic_publish() with:
-                        keep(it.topic_name == '/turtlebot2/cmd_vel')     
-                        keep(it.topic_type == 'geometry_msgs.msg.Twist')    
-                        keep(it.value == '{\"linear\": {\"x\": 0.30, \"y\": 0.0, \"z\": 0.0}, \"angular\": {\"x\": 0.0, \"y\": 0.0, \"z\": 0.0}}') 
-                    wait elapsed(0.2s)
-                    topic_publish() with:
-                        keep(it.topic_name == '/turtlebot2/cmd_vel')     
-                        keep(it.topic_type == 'geometry_msgs.msg.Twist')    
-                        keep(it.value == '{\"linear\": {\"x\": 0.30, \"y\": 0.0, \"z\": 0.0}, \"angular\": {\"x\": 0.0, \"y\": 0.0, \"z\": 0.0}}') 
-                    wait elapsed(0.2s)
-                    topic_publish() with:
-                        keep(it.topic_name == '/turtlebot2/cmd_vel')     
-                        keep(it.topic_type == 'geometry_msgs.msg.Twist')    
-                        keep(it.value == '{\"linear\": {\"x\": 0.30, \"y\": 0.0, \"z\": 0.0}, \"angular\": {\"x\": 0.0, \"y\": 0.0, \"z\": 0.0}}') 
-                    wait elapsed(0.2s)
-                    topic_publish() with:
-                        keep(it.topic_name == '/turtlebot2/cmd_vel')     
-                        keep(it.topic_type == 'geometry_msgs.msg.Twist')    
-                        keep(it.value == '{\"linear\": {\"x\": 0.30, \"y\": 0.0, \"z\": 0.0}, \"angular\": {\"x\": 0.0, \"y\": 0.0, \"z\": 0.0}}') 
-                    wait elapsed(0.2s)
-                    topic_publish() with:
-                        keep(it.topic_name == '/turtlebot2/cmd_vel')     
-                        keep(it.topic_type == 'geometry_msgs.msg.Twist')    
-                        keep(it.value == '{\"linear\": {\"x\": 0.30, \"y\": 0.0, \"z\": 0.0}, \"angular\": {\"x\": 0.0, \"y\": 0.0, \"z\": 0.0}}') 
-                    wait elapsed(0.2s)
-                    topic_publish() with:
-                        keep(it.topic_name == '/turtlebot2/cmd_vel')     
-                        keep(it.topic_type == 'geometry_msgs.msg.Twist')    
-                        keep(it.value == '{\"linear\": {\"x\": 0.30, \"y\": 0.0, \"z\": 0.0}, \"angular\": {\"x\": 0.0, \"y\": 0.0, \"z\": 0.0}}') 
-                    wait elapsed(0.2s)
-                    topic_publish() with:
-                        keep(it.topic_name == '/turtlebot2/cmd_vel')     
-                        keep(it.topic_type == 'geometry_msgs.msg.Twist')    
-                        keep(it.value == '{\"linear\": {\"x\": 0.30, \"y\": 0.0, \"z\": 0.0}, \"angular\": {\"x\": 0.0, \"y\": 0.0, \"z\": 0.0}}') 
-                    wait elapsed(0.2s)
-                    topic_publish() with:
-                        keep(it.topic_name == '/turtlebot2/cmd_vel')     
-                        keep(it.topic_type == 'geometry_msgs.msg.Twist')    
-                        keep(it.value == '{\"linear\": {\"x\": 0.30, \"y\": 0.0, \"z\": 0.0}, \"angular\": {\"x\": 0.0, \"y\": 0.0, \"z\": 0.0}}') 
-                    wait elapsed(0.2s)
-                    topic_publish() with:
-                        keep(it.topic_name == '/turtlebot2/cmd_vel')     
-                        keep(it.topic_type == 'geometry_msgs.msg.Twist')    
-                        keep(it.value == '{\"linear\": {\"x\": 0.30, \"y\": 0.0, \"z\": 0.0}, \"angular\": {\"x\": 0.0, \"y\": 0.0, \"z\": 0.0}}') 
-            emit end
+                        keep(it.value == '{\"linear\": {\"x\": 0.6, \"y\": 0.0, \"z\": 0.0}, \"angular\": {\"x\": 0.0, \"y\": 0.0, \"z\": 0.0}}')
+                    wait elapsed(10s)
         time_out: serial:
             wait elapsed(240s)
             emit fail

examples/example_multi_robot/launch/robot2_launch.py

@@ -0,0 +1,58 @@
+# 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
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+
+from launch_ros.actions import Node
+
+
+ARGUMENTS = [
+    DeclareLaunchArgument('robot_name', default_value='robot2',
+                          description='name of robot'),
+]
+
+
+def generate_launch_description():
+
+    # Launch configurations
+    robot_name = LaunchConfiguration('robot_name')
+
+    cmd_vel_bridge = Node(
+        package='ros_gz_bridge',
+        executable='parameter_bridge',
+        name='cmd_vel_bridge',
+        output='screen',
+        parameters=[{
+            'use_sim_time': True
+        }],
+        arguments=[
+            [robot_name,
+             '/cmd_vel' + '@geometry_msgs/msg/Twist' + '[ignition.msgs.Twist'],
+            ['/model/', robot_name, '/cmd_vel' +
+             '@geometry_msgs/msg/Twist' +
+             ']ignition.msgs.Twist']
+        ],
+        remappings=[
+            (['/model/', robot_name, '/cmd_vel'], [robot_name, '/cmd_vel'])
+        ]
+    )
+
+    # Define LaunchDescription variable
+    ld = LaunchDescription(ARGUMENTS)
+    ld.add_action(cmd_vel_bridge)
+    return ld