Add basic support for Modifiers (#110)

.devcontainer/deb_requirements_humble.txt

@@ -27,6 +27,7 @@
  ros-humble-navigation2
  ros-humble-nav2-bringup
  ros-humble-nav2-simple-commander
+ ros-humble-py-trees
  ros-humble-py-trees-ros-interfaces
  ros-humble-rcl-interfaces
  ros-humble-rclpy

docs/libraries.rst

@@ -209,7 +209,7 @@ For debugging purposes, log a string using the available log mechanism.
      - String to log
 
 ``run_process()``
-""""""""""""""""""""""""""
+"""""""""""""""""
 
 Run a process. Reports `running` while the process has not finished.
 
@@ -241,38 +241,82 @@ If ``wait_for_shutdown`` is ``false`` and the process is still running on scenar
      - ``10s``
      - (Only used if ``wait_for_shutdown`` is ``false``) time to wait between ``shutdown_signal`` and SIGKILL getting sent, if process is still running on scenario shutdown
 
-OS
---
 
-The library contains actions to interact with the operating system. Import it with ``import osc.os``. It is provided by the package :repo_link:`libs/scenario_execution_os`.
+Modifiers
+^^^^^^^^^
 
-Actions
-^^^^^^^
+``inverter()``
+""""""""""""""
 
-``check_file_exists()``
-"""""""""""""""""""""""
+Modifier to invert the action result. A failing action will report ``success``, a succeeding action will report ``failure``.
 
-Report success if a file exists.
+``repeat()``
+""""""""""""
+Modifier to repeat a sub-tree. If any of the included children report ``failure``, the repetition stops and ``failure`` is reported.
 
 .. list-table:: 
    :widths: 15 15 5 65
    :header-rows: 1
    :class: tight-table   
-   
+
    * - Parameter
      - Type
      - Default
      - Description
-   * - ``file_name``
-     - ``string``
-     -
-     - File to check
+   * - ``count``
+     - ``int``
+     - ``-1``
+     - Repeat this many times (-1 to repeat indefinitely)
+
+``retry()``
+"""""""""""
+Modifier to retry a sub-tree until it succeeds.
+
+.. list-table:: 
+   :widths: 15 15 5 65
+   :header-rows: 1
+   :class: tight-table   
+
+   * - Parameter
+     - Type
+     - Default
+     - Description
+   * - ``count``
+     - ``int``
+     - 
+     - Maximum number of permitted failures
+
+``timeout()``
+"""""""""""""
+Modifier to set a timeout for a sub-tree.
 
+.. list-table:: 
+   :widths: 15 15 5 65
+   :header-rows: 1
+   :class: tight-table   
 
-``check_file_not_exists()``
-"""""""""""""""""""""""""""
+   * - Parameter
+     - Type
+     - Default
+     - Description
+   * - ``count``
+     - ``int``
+     - 
+     - Maximum number of permitted failures
 
-Report success if a file does not exist.
+
+OS
+--
+
+The library contains actions to interact with the operating system. Import it with ``import osc.os``. It is provided by the package :repo_link:`libs/scenario_execution_os`.
+
+Actions
+^^^^^^^
+
+``check_file_exists()``
+"""""""""""""""""""""""
+
+Report success if a file exists.
 
 .. list-table:: 
    :widths: 15 15 5 65

docs/openscenario2.rst

@@ -14,7 +14,31 @@ The `standard library of
 OSC2 <https://www.asam.net/static_downloads/public/asam-openscenario/2.0.0/domain-model/standard_library.html>`__
 was adapted to be usable by the current parsing support of scenario execution.
 
-In the following the supported features are described.
+
+Mapping to py-trees
+-------------------
+
+.. list-table:: 
+   :widths: 15 25 60
+   :header-rows: 1
+   :class: tight-table   
+
+   - * OpenScenario2  
+     * py-trees
+     * Comment
+   - * ``action``
+     * ``Behaviour``
+     * Actions are derived from ``scenario_execution.actions.base_action.BaseAction`` which is derived from ``py_trees.behaviour.Behaviour``
+   - * ``event``
+     * blackboard entry and ``Behaviour``
+     * ``Behaviour`` is used to read and write blackboard variable
+   - * ``modifier``
+     * ``Decorator``
+     *
+   - * ``var``
+     * blackboard entry
+     * Variables are stored within the blackboard
+
 
 .. role:: raw-html(raw)
    :format: html
@@ -87,13 +111,13 @@ Composition Types
 
 Composition types are ``struct``, ``actor``, ``action``, ``scenario``.
 
-============== ==================== =========
+============== ==================== ===========================
 Element Type   Support              Notes
-============== ==================== =========
+============== ==================== ===========================
 Event          :raw-html:`&#9989;`      
 Field          :raw-html:`&#9989;`     
 Constraint     :raw-html:`&#9989;`  partially
-Method         :raw-html:`&#10060;`       
+Method         :raw-html:`&#9989;`       
 Coverage       :raw-html:`&#10060;`       
-Modifier       :raw-html:`&#10060;`       
-============== ==================== =========
+Modifier       :raw-html:`&#9989;`  partially (only predefined)     
+============== ==================== ===========================

docs/tutorials.rst

@@ -48,17 +48,14 @@ screen. After the ``log`` action is invoked, the ``wait`` directive makes the sc
    Scenario execution uses the predefined events ``end`` and ``fail`` to detect success or failure of a scenario. If no ``emit end`` or ``emit fail`` is defined, a success is assumed.
 
 .. note::
-    It is good practice to define a timeout action in parallel to the expected actions within a scenario.
+    It is good practice to define a timeout modifier within a scenario to avoid that failing scenarios run forever.
 
     .. code-block::
         
         scenario example:
-            do parallel:
-                serial:
-                    ...
-                serial:
-                    wait elapsed(60s)
-                    emit fail
+            timeout(60s)
+            do serial:
+                ...
 
 Use this code to see a launch of this tutorial:
 
@@ -442,4 +439,4 @@ Use this code to see a launch of this tutorial:
 .. code-block:: bash
 
    colcon build --packages-up-to example_external_method && source install/setup.bash \
-   && ros2 run scenario_execution scenario_execution examples/example_external_method/scenarios/example_external_method.osc
+   && ros2 run scenario_execution scenario_execution examples/example_external_method/scenarios/example_external_method.osc

examples/example_multi_robot/example_multi_robot.osc

@@ -1,31 +1,23 @@
+import osc.helpers
 import osc.ros
 import osc.gazebo
 
 scenario multi_robot:
+    timeout(240s)
     robot: differential_drive_robot
     robot2: osc_actor
-    do parallel:
-        serial:
-            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(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 == '/robot2/cmd_vel')
-                        keep(it.topic_type == 'geometry_msgs.msg.Twist')
-                        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
+    do serial:
+        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(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:
+                topic_publish() with:
+                    keep(it.topic_name == '/robot2/cmd_vel')
+                    keep(it.topic_type == 'geometry_msgs.msg.Twist')
+                    keep(it.value == '{\"linear\": {\"x\": 0.6, \"y\": 0.0, \"z\": 0.0}, \"angular\": {\"x\": 0.0, \"y\": 0.0, \"z\": 0.0}}')

examples/example_multi_robot/launch/robot2_launch.py

@@ -20,16 +20,13 @@
 
 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(
@@ -52,7 +49,6 @@ def generate_launch_description():
         ]
     )
 
-    # Define LaunchDescription variable
     ld = LaunchDescription(ARGUMENTS)
     ld.add_action(cmd_vel_bridge)
     return ld

examples/example_multi_robot/models/robot.sdf

@@ -1,9 +1,8 @@
 <?xml version="1.0" ?>
 <sdf version="1.8">
     <model name='vehicle_blue' canonical_link='chassis'>
-
         <link name='chassis'>
-            <inertial> <!--inertial properties of the link mass, inertia matix-->
+            <inertial>
                 <mass>10.14395</mass>
                 <inertia>
                     <ixx>0.126164</ixx>
@@ -17,28 +16,25 @@
             <visual name='visual'>
                 <geometry>
                     <box>
-                        <size>0.5 0.4 0.3</size> <!--question: this size is in meter-->
+                        <size>0.5 0.4 0.3</size>
                     </box>
                 </geometry>
-                <!--let's add color to our link-->
                 <material>
                     <ambient>0.0 0.0 1.0 1</ambient>
                     <diffuse>0.0 0.0 1.0 1</diffuse>
                     <specular>0.0 0.0 1.0 1</specular>
                 </material>
             </visual>
-            <collision name='collision'> <!--todo: describe why we need the collision-->
+            <collision name='collision'>
                 <geometry>
                     <box>
                         <size>0.5 0.4 0.3</size>
                     </box>
                 </geometry>
             </collision>
         </link>
-
-        <!--let's build the left wheel-->
         <link name='left_wheel'>
-            <pose relative_to="chassis">-0.15 0.225 -0.1 -1.5707 0 0</pose> <!--angles are in radian-->
+            <pose relative_to="chassis">-0.15 0.225 -0.1 -1.5707 0 0</pose>
             <inertial>
                 <mass>2</mass>
                 <inertia>
@@ -72,10 +68,8 @@
                 </geometry>
             </collision>
         </link>
-
-        <!--copy and paste for right wheel but change position-->
         <link name='right_wheel'>
-            <pose relative_to="chassis">-0.15 -0.225 -0.1 -1.5707 0 0</pose> <!--angles are in radian-->
+            <pose relative_to="chassis">-0.15 -0.225 -0.1 -1.5707 0 0</pose>
             <inertial>
                 <mass>1</mass>
                 <inertia>
@@ -109,12 +103,9 @@
                 </geometry>
             </collision>
         </link>
-
         <frame name="caster_frame" attached_to='chassis'>
             <pose>0.15 0 -0.15 0 0 0</pose>
         </frame>
-
-        <!--caster wheel-->
         <link name='caster'>
             <pose relative_to='caster_frame'/>
             <inertial>
@@ -148,42 +139,34 @@
                 </geometry>
             </collision>
         </link>
-
-
-        <!--connecting these links together using joints-->
-        <joint name='left_wheel_joint' type='revolute'> <!--continous joint is not supported yet-->
+        <joint name='left_wheel_joint' type='revolute'>
             <pose relative_to='left_wheel'/>
             <parent>chassis</parent>
             <child>left_wheel</child>
             <axis>
-                <xyz expressed_in='__model__'>0 1 0</xyz> <!--can be defined as any frame or even arbitrary frames-->
+                <xyz expressed_in='__model__'>0 1 0</xyz>
                 <limit>
-                    <lower>-1.79769e+308</lower>    <!--negative infinity-->
-                    <upper>1.79769e+308</upper>     <!--positive infinity-->
+                    <lower>-1.79769e+308</lower>
+                    <upper>1.79769e+308</upper>
                 </limit>
             </axis>
         </joint>
-
         <joint name='right_wheel_joint' type='revolute'>
             <pose relative_to='right_wheel'/>
             <parent>chassis</parent>
             <child>right_wheel</child>
             <axis>
                 <xyz expressed_in='__model__'>0 1 0</xyz>
                 <limit>
-                    <lower>-1.79769e+308</lower>    <!--negative infinity-->
-                    <upper>1.79769e+308</upper>     <!--positive infinity-->
+                    <lower>-1.79769e+308</lower>
+                    <upper>1.79769e+308</upper>
                 </limit>
             </axis>
         </joint>
-
-        <!--different type of joints ball joint--> <!--defult value is the child-->
         <joint name='caster_wheel' type='ball'>
             <parent>chassis</parent>
             <child>caster</child>
         </joint>
-
-        <!--diff drive plugin-->
         <plugin
             filename="gz-sim-diff-drive-system"
             name="gz::sim::systems::DiffDrive">