Support usage of ros messages as parameters (#165)

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

scenario_execution/scenario_execution/model/error.py

@@ -18,24 +18,36 @@
 from antlr4 import ParserRuleContext
 
 
-class OSC2ParsingError(Exception):
-    """
-    Error class for OSC2 parser
-    """
+class OSC2Error(Exception):
 
     def __init__(self, msg: str, context, *args) -> None:
         super().__init__(*args)
         self.msg = msg
         if isinstance(context, ParserRuleContext):
-            self.line = context.start.line
-            self.column = context.start.column
-            self.context = context.getText()
-            self.filename = ""
+            self.osc_ctx = (context.start.line, context.start.column, context.getText(), "")
         else:
-            self.line = context[0]
-            self.column = context[1]
-            self.context = context[2]
-            self.filename = context[3]
+            self.osc_ctx = context
 
     def __str__(self) -> str:
-        return self.msg
+        error_str = ""
+        if self.osc_ctx is not None:
+            if len(self.osc_ctx) == 4:
+                context = self.osc_ctx[2].replace('\n', '')
+                error_str = f"(line: {self.osc_ctx[0]}, column: {self.osc_ctx[1]} in '{self.osc_ctx[3]}') -> {context}: "
+            else:
+                error_str = f"<invalid context: {self.osc_ctx}>: "
+        error_str += self.msg
+        return error_str
+
+
+class OSC2ParsingError(OSC2Error):
+    """
+    Error class for OSC2 parser
+    """
+
+    def __init__(self, msg: str, context, *args) -> None:
+        if isinstance(context, ParserRuleContext):
+            ctx = (context.start.line, context.start.column, context.getText(), "")
+        else:
+            ctx = context
+        super().__init__(msg, ctx, *args)

scenario_execution/scenario_execution/model/model_blackboard.py

@@ -26,8 +26,7 @@ def create_py_tree_blackboard(model, tree, logger, log_tree):
     try:
         model_blackboard.build(model, tree, log_tree)
     except OSC2ParsingError as e:
-        raise ValueError(
-            f'Error while creating py-tree:\nTraceback <line: {e.line}, column: {e.column}> in "{e.filename}":\n  -> {e.context}\n{e.__class__.__name__}: {e.msg}') from e
+        raise ValueError(f'Error while creating py-tree: {e}') from e
 
 
 class ModelToBlackboard(object):

scenario_execution/scenario_execution/model/model_resolver.py

@@ -29,8 +29,7 @@ def resolve_internal_model(model, tree, logger, log_tree):
     try:
         osc2scenario_resolver.visit(model)
     except OSC2ParsingError as e:
-        raise ValueError(
-            f'Error while creating tree:\nTraceback <line: {e.line}, column: {e.column}> in "{e.filename}":\n  -> {e.context}\n{e.__class__.__name__}: {e.msg}') from e
+        raise ValueError(f'Error while creating tree: {e}') from e
 
     if log_tree:
         logger.info("----Internal model (resolved)-----")

scenario_execution/scenario_execution/model/model_to_py_tree.py

@@ -31,8 +31,7 @@ def create_py_tree(model, tree, logger, log_tree):
     try:
         final_tree = model_to_py_tree.build(model, tree, log_tree)
     except OSC2ParsingError as e:
-        raise ValueError(
-            f'Error while creating py-tree:\nTraceback <line: {e.line}, column: {e.column}> in "{e.filename}":\n  -> {e.context}\n{e.__class__.__name__}: {e.msg}') from e
+        raise ValueError(f'Error while creating py-tree: {e}') from e
     return final_tree
 
 
@@ -102,15 +101,15 @@ def update(self):
         return Status.SUCCESS
 
 
-class ExpressionBehavior(py_trees.behaviour.Behaviour):
-
-    def __init__(self, name: "ExpressionBehavior", expression: Expression):
-        super().__init__(name)
+class ExpressionBehavior(BaseAction):  # py_trees.behaviour.Behaviour):
 
+    def __init__(self, name: "ExpressionBehavior", expression: Expression, model, logger):
+        super().__init__()
+        self._set_base_properities(name, model, logger)
         self.expression = expression
 
     def update(self):
-        if self.expression.eval():
+        if self.expression.eval(self.get_blackboard_client()):
             return Status.SUCCESS
         else:
             return Status.RUNNING
@@ -215,7 +214,7 @@ def compare_method_arguments(self, method, expected_args, behavior_name, node):
                 if error_string:
                     error_string += ", "
                 error_string += "unknown: " + ", ".join(unknown_args)
-            return method_args, error_string
+            return method_args, error_string, missing_args
 
         def create_decorator(self, node: ModifierDeclaration, resolved_values):
             available_modifiers = ["repeat", "inverter", "timeout", "retry", "failure_is_running", "failure_is_success",
@@ -311,18 +310,19 @@ def visit_behavior_invocation(self, node: BehaviorInvocation):
                     # if __init__() is defined, check parameters. Allowed:
                     # - __init__(self)
                     # - __init__(self, resolve_variable_reference_arguments_in_execute)
-                    # - __init__(self, <all-osc-defined-args>)
-                    init_args, error_string = self.compare_method_arguments(init_method, expected_args, behavior_name, node)
+                    # - __init__(self, <some-or-all-osc-defined-args>)
+                    init_args, error_string, args_not_in_init = self.compare_method_arguments(
+                        init_method, expected_args, behavior_name, node)
                     if init_args != ["self"] and \
                             init_args != ["self", "resolve_variable_reference_arguments_in_execute"] and \
-                            set(init_args) != set(expected_args):
+                            not all(x in expected_args for x in init_args):
                         raise OSC2ParsingError(
-                            msg=f'Plugin {behavior_name}: __init__() either only has "self" argument or all arguments defined in osc. {error_string}\n'
+                            msg=f'Plugin {behavior_name}: __init__() either only has "self" argument and osc-defined arguments. {error_string}\n'
                                 f'expected definition with all arguments: {expected_args}', context=node.get_ctx()
                         )
                 execute_method = getattr(behavior_cls, "execute", None)
                 if execute_method is not None:
-                    _, error_string = self.compare_method_arguments(execute_method, expected_args, behavior_name, node)
+                    _, error_string, _ = self.compare_method_arguments(execute_method, expected_args, behavior_name, node)
                     if error_string:
                         raise OSC2ParsingError(
                             msg=f'Plugin {behavior_name}: execute() arguments differ from osc-definition: {error_string}.', context=node.get_ctx()
@@ -336,12 +336,14 @@ def visit_behavior_invocation(self, node: BehaviorInvocation):
                     f"Instantiate action '{action_name}', plugin '{behavior_name}'. with:\nExpected execute() arguments: {expected_args}")
                 try:
                     if init_args is not None and init_args != ['self'] and init_args != ['self', 'resolve_variable_reference_arguments_in_execute']:
-                        final_args = node.get_resolved_value(self.blackboard)
+                        final_args = node.get_resolved_value(self.blackboard, skip_keys=args_not_in_init)
 
                         if node.actor:
                             final_args["associated_actor"] = node.actor.get_resolved_value(self.blackboard)
                             final_args["associated_actor"]["name"] = node.actor.name
 
+                        for k in args_not_in_init:
+                            del final_args[k]
                         instance = behavior_cls(**final_args)
                     else:
                         instance = behavior_cls()
@@ -363,7 +365,7 @@ def visit_event_condition(self, node: EventCondition):
             expression = ""
             for child in node.get_children():
                 if isinstance(child, (RelationExpression, LogicalExpression)):
-                    expression = ExpressionBehavior(name=node.get_ctx()[2], expression=self.visit(child))
+                    expression = ExpressionBehavior(name=node.get_ctx()[2], expression=self.visit(child), model=node, logger=self.logger)
                 elif isinstance(child, ElapsedExpression):
                     elapsed_condition = self.visit_elapsed_expression(child)
                     expression = py_trees.timers.Timer(name=f"wait {elapsed_condition}s", duration=float(elapsed_condition))