Support usage of ros messages as parameters (#165)

Author: fred-labs

docs/development.rst

@@ -95,6 +95,8 @@ Implement an Action
    - Make use of ``kwargs['logger']``, available in ``setup()``
    - If you want to draw markers for RViz, use ``kwargs['marker_handler']``, available in ``setup()`` (with ROS backend)
 - Use arguments from ``__init__()`` for a longer running initialization in ``setup()`` and the arguments from ``execute()`` to set values just before executing the action.
+  - ``__init__()`` does not need to contain all osc2-defined arguments. This can be convenient as variable argument resolving might not be available during ``__init__()``.
+  - ``execute()`` contains all osc2-arguments.
 - ``setup()`` provides several arguments that might be useful:
   - ``input_dir``: Directory containing the scenario file
   - ``output_dir``: If given on command-line, contains the directory to save output to

libs/scenario_execution_floorplan_dsl/scenario_execution_floorplan_dsl/actions/generate_floorplan.py

@@ -18,7 +18,7 @@
 from enum import Enum
 
 import py_trees
-from scenario_execution.actions.base_action import BaseAction
+from scenario_execution.actions.base_action import BaseAction, ActionError
 
 import docker
 import tempfile
@@ -46,20 +46,20 @@ def setup(self, **kwargs):
         # self.output_dir = tempfile.mkdtemp() # for testing: does not remove directory afterwards
 
         if "input_dir" not in kwargs:
-            raise ValueError("input_dir not defined.")
+            raise ActionError("input_dir not defined.", action=self)
         input_dir = kwargs["input_dir"]
         # check docker image
         self.client = docker.from_env()
         image_name = 'floorplan:latest'
         filterred_images = self.client.images.list(filters={'reference': image_name})
         if len(filterred_images) == 0:
-            raise ValueError(f"Required docker image '{image_name}' does not exist.")
+            raise ActionError(f"Required docker image '{image_name}' does not exist.", action=self)
 
         # check files
         if not os.path.isabs(self.file_path):
             self.file_path = os.path.join(input_dir, self.file_path)
         if not os.path.isfile(self.file_path):
-            raise ValueError(f"Floorplan file {self.file_path} not found.")
+            raise ActionError(f"Floorplan file {self.file_path} not found.", action=self)
         self.floorplan_name = os.path.splitext(os.path.basename(self.file_path))[0]
 
     def update(self) -> py_trees.common.Status:

libs/scenario_execution_floorplan_dsl/scenario_execution_floorplan_dsl/actions/generate_gazebo_world.py

@@ -17,7 +17,7 @@
 import os
 import py_trees
 from scenario_execution_gazebo.actions.utils import SpawnUtils
-from scenario_execution.actions.base_action import BaseAction
+from scenario_execution.actions.base_action import BaseAction, ActionError
 from shutil import which
 import tempfile
 
@@ -31,15 +31,15 @@ def __init__(self, associated_actor, sdf_template: str, arguments: list):
 
     def setup(self, **kwargs):
         if which("xacro") is None:
-            raise ValueError("'xacro' not found.")
+            raise ActionError("'xacro' not found.", action=self)
         if "input_dir" not in kwargs:
-            raise ValueError("input_dir not defined.")
+            raise ActionError("input_dir not defined.", action=self)
         input_dir = kwargs["input_dir"]
 
         if not os.path.isabs(self.sdf_template):
             self.sdf_template = os.path.join(input_dir, self.sdf_template)
         if not os.path.isfile(self.sdf_template):
-            raise ValueError(f"SDF Template {self.sdf_template} not found.")
+            raise ActionError(f"SDF Template {self.sdf_template} not found.", action=self)
         self.tmp_file = tempfile.NamedTemporaryFile(suffix=".sdf")  # for testing, do not delete temp file: delete=False
 
     def execute(self, associated_actor, sdf_template: str, arguments: list):

libs/scenario_execution_gazebo/scenario_execution_gazebo/actions/gazebo_relative_spawn_actor.py

@@ -21,6 +21,7 @@
 from tf2_ros.transform_listener import TransformListener
 from tf2_geometry_msgs import PoseStamped
 from .gazebo_spawn_actor import GazeboSpawnActor
+from scenario_execution.actions.base_action import ActionError
 
 
 class GazeboRelativeSpawnActor(GazeboSpawnActor):
@@ -97,4 +98,4 @@ def calculate_new_pose(self):
                 f' w: {new_pose.pose.orientation.w} x: {new_pose.pose.orientation.x} y: {new_pose.pose.orientation.y} z: {new_pose.pose.orientation.z}' \
                 ' } }'
         except TransformException as e:
-            raise ValueError(f"No transform available ({self.parent_frame_id}->{self.frame_id})") from e
+            raise ActionError(f"No transform available ({self.parent_frame_id}->{self.frame_id})", action=self) from e

libs/scenario_execution_gazebo/scenario_execution_gazebo/actions/gazebo_spawn_actor.py

@@ -23,6 +23,7 @@
 from rclpy.qos import QoSProfile, QoSDurabilityPolicy, QoSHistoryPolicy, QoSReliabilityPolicy
 from rclpy.node import Node
 import py_trees
+from scenario_execution.actions.base_action import ActionError
 from scenario_execution.actions.run_process import RunProcess
 from .utils import SpawnUtils
 
@@ -68,7 +69,7 @@ def setup(self, **kwargs):
         except KeyError as e:
             error_message = "didn't find 'node' in setup's kwargs [{}][{}]".format(
                 self.name, self.__class__.__name__)
-            raise KeyError(error_message) from e
+            raise ActionError(error_message, action=self) from e
 
         self.utils = SpawnUtils(logger=self.logger)
 
@@ -88,12 +89,12 @@ def setup(self, **kwargs):
                 self.entity_model, self.entity_name, self.xacro_arguments)
 
             if not self.sdf:
-                raise ValueError(f'Invalid model specified ({self.entity_model})')
+                raise ActionError(f'Invalid model specified ({self.entity_model})', action=self)
             self.current_state = SpawnActionState.MODEL_AVAILABLE
 
     def execute(self, associated_actor, spawn_pose: list, world_name: str, xacro_arguments: list, model: str):  # pylint: disable=arguments-differ
         if self.entity_model != model or set(self.xacro_arguments) != set(xacro_arguments):
-            raise ValueError("Runtime change of model not supported.")
+            raise ActionError("Runtime change of model not supported.", action=self)
         self.spawn_pose = spawn_pose
         self.world_name = world_name
 
@@ -175,7 +176,7 @@ def get_spawn_pose(self):
                 f' w: {quaternion[0]} x: {quaternion[1]} y: {quaternion[2]} z: {quaternion[3]}' \
                 ' } }'
         except KeyError as e:
-            raise ValueError("Could not get values") from e
+            raise ActionError("Could not get values", action=self) from e
         return pose
 
     def set_command(self, command):

libs/scenario_execution_nav2/scenario_execution_nav2/actions/init_nav2.py

@@ -31,7 +31,7 @@
 
 from .nav2_common import NamespaceAwareBasicNavigator
 from scenario_execution_ros.actions.common import get_pose_stamped, NamespacedTransformListener
-from scenario_execution.actions.base_action import BaseAction
+from scenario_execution.actions.base_action import BaseAction, ActionError
 
 
 class InitNav2State(Enum):
@@ -91,7 +91,7 @@ def setup(self, **kwargs):
         except KeyError as e:
             error_message = "didn't find 'node' in setup's kwargs [{}][{}]".format(
                 self.name, self.__class__.__name__)
-            raise KeyError(error_message) from e
+            raise ActionError(error_message, action=self) from e
 
         self.tf_buffer = Buffer()
         self.tf_listener = NamespacedTransformListener(

libs/scenario_execution_pybullet/scenario_execution_pybullet/actions/sim_run.py

@@ -15,7 +15,7 @@
 # SPDX-License-Identifier: Apache-2.0
 
 import py_trees
-from scenario_execution.actions.base_action import BaseAction
+from scenario_execution.actions.base_action import BaseAction, ActionError
 import pybullet as p
 import math
 
@@ -30,10 +30,10 @@ def setup(self, **kwargs):
         try:
             tick_period: float = kwargs['tick_period']
         except KeyError as e:
-            raise KeyError("didn't find 'tick_period' in setup's kwargs") from e
+            raise ActionError("didn't find 'tick_period' in setup's kwargs", action=self) from e
         if not math.isclose(240 % tick_period, 0., abs_tol=1e-4):
-            raise ValueError(
-                f"Scenario Execution Tick Period of {tick_period} is not compatible with PyBullet stepping. Please set step-duration to be a multiple of 1/240s")
+            raise ActionError(
+                f"Scenario Execution Tick Period of {tick_period} is not compatible with PyBullet stepping. Please set step-duration to be a multiple of 1/240s", action=self)
         self.sim_steps_per_tick = round(240 * tick_period)
         self.logger.info(f"Forward simulation by {self.sim_steps_per_tick} step per scenario tick.")
 

libs/scenario_execution_x11/scenario_execution_x11/actions/capture_screen.py

@@ -17,6 +17,7 @@
 from enum import Enum
 import py_trees
 import os
+from scenario_execution.actions.base_action import ActionError
 from scenario_execution.actions.run_process import RunProcess
 
 
@@ -36,12 +37,12 @@ def __init__(self, output_filename: str, frame_rate: float):
 
     def setup(self, **kwargs):
         if "DISPLAY" not in os.environ:
-            raise ValueError("capture_screen() requires environment variable 'DISPLAY' to be set.")
+            raise ActionError("capture_screen() requires environment variable 'DISPLAY' to be set.", action=self)
 
         if kwargs['output_dir']:
             if not os.path.exists(kwargs['output_dir']):
-                raise ValueError(
-                    f"Specified destination dir '{kwargs['output_dir']}' does not exist")
+                raise ActionError(
+                    f"Specified destination dir '{kwargs['output_dir']}' does not exist", action=self)
             self.output_dir = kwargs['output_dir']
 
     def execute(self, output_filename: str, frame_rate: float):  # pylint: disable=arguments-differ

scenario_execution/scenario_execution/actions/base_action.py

@@ -16,6 +16,7 @@
 
 import py_trees
 from scenario_execution.model.types import ParameterDeclaration, ScenarioDeclaration
+from scenario_execution.model.error import OSC2Error
 
 
 class BaseAction(py_trees.behaviour.Behaviour):
@@ -78,7 +79,7 @@ def get_blackboard_namespace(node: ParameterDeclaration):
 
     def register_access_to_associated_actor_variable(self, variable_name):
         if not self._model.actor:
-            raise ValueError("Model does not have 'actor'.")
+            raise ActionError("Model does not have 'actor'.", action=self)
         blackboard = self.get_blackboard_client()
         model_blackboard_name = self._model.actor.get_fully_qualified_var_name(include_scenario=False)
         model_blackboard_name += "/" + variable_name
@@ -94,3 +95,11 @@ def get_associated_actor_variable(self, variable_name):
         model_blackboard_name = self.register_access_to_associated_actor_variable(variable_name)
         self.logger.debug(f"Get variable '{model_blackboard_name}'")
         return getattr(self.get_blackboard_client(), model_blackboard_name)
+
+
+class ActionError(OSC2Error):
+
+    def __init__(self, msg: str, action: BaseAction, *args) -> None:
+        if action is not None:
+            ctx = action._model.get_ctx()
+        super().__init__(msg, ctx, *args)

scenario_execution/scenario_execution/actions/log.py

@@ -16,7 +16,7 @@
 
 import py_trees
 from py_trees.common import Status
-from scenario_execution.actions.base_action import BaseAction
+from scenario_execution.actions.base_action import BaseAction, ActionError
 
 
 class Log(BaseAction):
@@ -38,7 +38,7 @@ def update(self) -> py_trees.common.Status:
         if not self.published:
             self.published = True
             if not self.msg:
-                raise ValueError("log(): Empty message.")
+                raise ActionError("log(): Empty message.", action=self)
             self.logger.info(f"{self.msg}")
 
         return Status.SUCCESS