diff --git a/docs/development.rst b/docs/development.rst index 478254aa..2bbf9eeb 100644 --- a/docs/development.rst +++ b/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 diff --git a/libs/scenario_execution_floorplan_dsl/scenario_execution_floorplan_dsl/actions/generate_floorplan.py b/libs/scenario_execution_floorplan_dsl/scenario_execution_floorplan_dsl/actions/generate_floorplan.py index 5cb9df93..863e7f2c 100644 --- a/libs/scenario_execution_floorplan_dsl/scenario_execution_floorplan_dsl/actions/generate_floorplan.py +++ b/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: diff --git a/libs/scenario_execution_floorplan_dsl/scenario_execution_floorplan_dsl/actions/generate_gazebo_world.py b/libs/scenario_execution_floorplan_dsl/scenario_execution_floorplan_dsl/actions/generate_gazebo_world.py index c0be2116..c8dd7d24 100644 --- a/libs/scenario_execution_floorplan_dsl/scenario_execution_floorplan_dsl/actions/generate_gazebo_world.py +++ b/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): diff --git a/libs/scenario_execution_gazebo/scenario_execution_gazebo/actions/gazebo_relative_spawn_actor.py b/libs/scenario_execution_gazebo/scenario_execution_gazebo/actions/gazebo_relative_spawn_actor.py index d3da21d4..22ab2e8d 100644 --- a/libs/scenario_execution_gazebo/scenario_execution_gazebo/actions/gazebo_relative_spawn_actor.py +++ b/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 diff --git a/libs/scenario_execution_gazebo/scenario_execution_gazebo/actions/gazebo_spawn_actor.py b/libs/scenario_execution_gazebo/scenario_execution_gazebo/actions/gazebo_spawn_actor.py index 2d8fe1dc..7ea6b56d 100644 --- a/libs/scenario_execution_gazebo/scenario_execution_gazebo/actions/gazebo_spawn_actor.py +++ b/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): diff --git a/libs/scenario_execution_nav2/scenario_execution_nav2/actions/init_nav2.py b/libs/scenario_execution_nav2/scenario_execution_nav2/actions/init_nav2.py index 7c27c851..1bbeac9d 100644 --- a/libs/scenario_execution_nav2/scenario_execution_nav2/actions/init_nav2.py +++ b/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( diff --git a/libs/scenario_execution_pybullet/scenario_execution_pybullet/actions/sim_run.py b/libs/scenario_execution_pybullet/scenario_execution_pybullet/actions/sim_run.py index c4ba4688..358a3e62 100644 --- a/libs/scenario_execution_pybullet/scenario_execution_pybullet/actions/sim_run.py +++ b/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.") diff --git a/libs/scenario_execution_x11/scenario_execution_x11/actions/capture_screen.py b/libs/scenario_execution_x11/scenario_execution_x11/actions/capture_screen.py index 432c49aa..c28ab8eb 100644 --- a/libs/scenario_execution_x11/scenario_execution_x11/actions/capture_screen.py +++ b/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 diff --git a/scenario_execution/scenario_execution/actions/base_action.py b/scenario_execution/scenario_execution/actions/base_action.py index c4e254fb..3e5d8893 100644 --- a/scenario_execution/scenario_execution/actions/base_action.py +++ b/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) diff --git a/scenario_execution/scenario_execution/actions/log.py b/scenario_execution/scenario_execution/actions/log.py index b57d6d1a..4cc77f66 100644 --- a/scenario_execution/scenario_execution/actions/log.py +++ b/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 diff --git a/scenario_execution/scenario_execution/model/error.py b/scenario_execution/scenario_execution/model/error.py index 6c64d4bb..8d1c076f 100644 --- a/scenario_execution/scenario_execution/model/error.py +++ b/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": " + 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) diff --git a/scenario_execution/scenario_execution/model/model_blackboard.py b/scenario_execution/scenario_execution/model/model_blackboard.py index d2456522..de59e7d3 100644 --- a/scenario_execution/scenario_execution/model/model_blackboard.py +++ b/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 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): diff --git a/scenario_execution/scenario_execution/model/model_resolver.py b/scenario_execution/scenario_execution/model/model_resolver.py index 81118477..87ddfa81 100644 --- a/scenario_execution/scenario_execution/model/model_resolver.py +++ b/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 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)-----") diff --git a/scenario_execution/scenario_execution/model/model_to_py_tree.py b/scenario_execution/scenario_execution/model/model_to_py_tree.py index f5a0074a..cecda3b7 100644 --- a/scenario_execution/scenario_execution/model/model_to_py_tree.py +++ b/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 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, ) - init_args, error_string = self.compare_method_arguments(init_method, expected_args, behavior_name, node) + # - __init__(self, ) + 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)) diff --git a/scenario_execution/scenario_execution/model/osc2_parser.py b/scenario_execution/scenario_execution/model/osc2_parser.py index 94118ffb..1da88337 100644 --- a/scenario_execution/scenario_execution/model/osc2_parser.py +++ b/scenario_execution/scenario_execution/model/osc2_parser.py @@ -64,8 +64,7 @@ def load_internal_model(self, tree, file_name: str, log_model: bool = False, deb walker.walk(model_builder, tree) model = model_builder.get_model() except OSC2ParsingError as e: - raise ValueError( - f'Error creating internal model: Traceback in "{e.filename}":\n -> {e.context}\n{e.__class__.__name__}: {e.msg}') from e + raise ValueError(f'Error creating internal model: {e}') from e if log_model: self.logger.info("----Internal model-----") print_tree(model, self.logger) diff --git a/scenario_execution/scenario_execution/model/types.py b/scenario_execution/scenario_execution/model/types.py index 9903e826..9148ef76 100644 --- a/scenario_execution/scenario_execution/model/types.py +++ b/scenario_execution/scenario_execution/model/types.py @@ -394,7 +394,9 @@ def get_parameter_names(self): names.append(child.name) return list(set(names)) - def get_resolved_value(self, blackboard=None): + def get_resolved_value(self, blackboard=None, skip_keys=None): + if skip_keys is None: + skip_keys = [] params = {} # set values defined in base type @@ -406,6 +408,8 @@ def get_resolved_value(self, blackboard=None): param_keys = list(params.keys()) for child in self.get_children(): if isinstance(child, ParameterDeclaration): + if child.name in skip_keys: + continue # set from parameter param_type, _ = child.get_type() @@ -433,16 +437,19 @@ def get_resolved_value(self, blackboard=None): if named: raise OSC2ParsingError( msg=f'Positional argument after named argument not allowed.', context=child.get_ctx()) - params[param_keys[pos]] = child.get_resolved_value(blackboard) + if param_keys[pos] not in skip_keys: + params[param_keys[pos]] = child.get_resolved_value(blackboard) pos += 1 elif isinstance(child, NamedArgument): named = True - params[child.name] = child.get_resolved_value(blackboard) + if child.name not in skip_keys: + params[child.name] = child.get_resolved_value(blackboard) elif isinstance(child, KeepConstraintDeclaration): tmp = child.get_resolved_value(blackboard) merge_nested_dicts(params, tmp, key_must_exist=False) elif isinstance(child, MethodDeclaration): - params[child.name] = child.get_resolved_value(blackboard) + if child.name not in skip_keys: + params[child.name] = child.get_resolved_value(blackboard) return params @@ -1836,7 +1843,7 @@ def get_type_string(self): return type_string def get_resolved_value(self, blackboard=None): - return visit_expression(self, blackboard).eval() + return visit_expression(self, blackboard).eval(blackboard) class UnaryExpression(ModelExpression): @@ -1904,7 +1911,7 @@ def get_type_string(self): return "bool" def get_resolved_value(self, blackboard=None): - return visit_expression(self, blackboard).eval() + return visit_expression(self, blackboard).eval(blackboard) class RelationExpression(ModelExpression): @@ -1931,7 +1938,7 @@ def get_type_string(self): return "bool" def get_resolved_value(self, blackboard=None): - return visit_expression(self, blackboard).eval() + return visit_expression(self, blackboard).eval(blackboard) class ListExpression(ModelExpression): @@ -2272,20 +2279,22 @@ def __init__(self, left, right, operator) -> None: self.right = right self.operator = operator - def resolve(self, param): + def resolve(self, param, blackboard): if isinstance(param, Expression): - return param.eval() + return param.eval(blackboard) elif isinstance(param, VariableReference): return param.get_value() + elif isinstance(param, FunctionApplicationExpression): + return param.get_resolved_value(blackboard) else: return param - def eval(self): - left = self.resolve(self.left) + def eval(self, blackboard): + left = self.resolve(self.left, blackboard) if self.right is None: return self.operator(left) else: - right = self.resolve(self.right) + right = self.resolve(self.right, blackboard) return self.operator(left, right) @@ -2339,6 +2348,8 @@ def visit_expression(node, blackboard): args[idx] = var_def else: args[idx] = child.get_resolved_value(blackboard) + elif isinstance(child, FunctionApplicationExpression): + args[idx] = child else: args[idx] = child.get_resolved_value(blackboard) idx += 1 diff --git a/scenario_execution_ros/scenario_execution_ros/actions/assert_lifecycle_state.py b/scenario_execution_ros/scenario_execution_ros/actions/assert_lifecycle_state.py index 7138b38a..6701e418 100644 --- a/scenario_execution_ros/scenario_execution_ros/actions/assert_lifecycle_state.py +++ b/scenario_execution_ros/scenario_execution_ros/actions/assert_lifecycle_state.py @@ -19,7 +19,7 @@ from lifecycle_msgs.srv import GetState from lifecycle_msgs.msg import TransitionEvent from scenario_execution_ros.actions.conversions import get_qos_preset_profile -from scenario_execution.actions.base_action import BaseAction +from scenario_execution.actions.base_action import BaseAction, ActionError from enum import Enum from queue import Queue from collections import deque @@ -56,7 +56,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 topic_transition_event_name = "/" + self.node_name + "/transition_event" self.subscription = self.node.create_subscription( @@ -67,7 +67,7 @@ def setup(self, **kwargs): def execute(self, node_name: str, state_sequence: list, allow_initial_skip: bool, fail_on_unexpected: bool, keep_running: bool): if self.node_name != node_name or self.state_sequence != state_sequence: - raise ValueError("Runtime change of arguments 'name', 'state_sequence not supported.") + raise ActionError("Runtime change of arguments 'name', 'state_sequence not supported.", action=self) if all(isinstance(state, tuple) and len(state) == 2 for state in self.state_sequence): self.state_sequence = [state[0] for state in self.state_sequence] @@ -75,7 +75,7 @@ def execute(self, node_name: str, state_sequence: list, allow_initial_skip: bool allowed_states = ['unconfigured', 'inactive', 'active', 'finalized'] for value in self.state_sequence: if value not in allowed_states: - raise ValueError("The specified state_sequence is not valid") + raise ActionError("The specified state_sequence is not valid", action=self) self.state_sequence = deque(self.state_sequence) self.allow_initial_skip = allow_initial_skip self.fail_on_unexpected = fail_on_unexpected diff --git a/scenario_execution_ros/scenario_execution_ros/actions/assert_tf_moving.py b/scenario_execution_ros/scenario_execution_ros/actions/assert_tf_moving.py index 0e13c518..2b2164bc 100644 --- a/scenario_execution_ros/scenario_execution_ros/actions/assert_tf_moving.py +++ b/scenario_execution_ros/scenario_execution_ros/actions/assert_tf_moving.py @@ -20,7 +20,7 @@ import time import tf2_ros from .common import NamespacedTransformListener -from scenario_execution.actions.base_action import BaseAction +from scenario_execution.actions.base_action import BaseAction, ActionError from tf2_ros import TransformException # pylint: disable= no-name-in-module import math @@ -51,7 +51,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.feedback_message = f"Waiting for transform {self.parent_frame_id} --> {self.frame_id}" # pylint: disable= attribute-defined-outside-init self.tf_buffer = tf2_ros.Buffer() @@ -67,7 +67,7 @@ def setup(self, **kwargs): def execute(self, frame_id: str, parent_frame_id: str, timeout: int, threshold_translation: float, threshold_rotation: float, wait_for_first_transform: bool, tf_topic_namespace: str, use_sim_time: bool): if self.tf_topic_namespace != tf_topic_namespace: - raise ValueError("Runtime change of argument 'tf_topic_namespace' not supported.") + raise ActionError("Runtime change of argument 'tf_topic_namespace' not supported.", action=self) self.frame_id = frame_id self.parent_frame_id = parent_frame_id self.timeout = timeout diff --git a/scenario_execution_ros/scenario_execution_ros/actions/assert_topic_latency.py b/scenario_execution_ros/scenario_execution_ros/actions/assert_topic_latency.py index 601fade6..7ac038d7 100644 --- a/scenario_execution_ros/scenario_execution_ros/actions/assert_topic_latency.py +++ b/scenario_execution_ros/scenario_execution_ros/actions/assert_topic_latency.py @@ -21,7 +21,7 @@ import importlib import time from scenario_execution_ros.actions.conversions import get_comparison_operator, get_qos_preset_profile -from scenario_execution.actions.base_action import BaseAction +from scenario_execution.actions.base_action import BaseAction, ActionError class AssertTopicLatency(BaseAction): @@ -51,17 +51,17 @@ 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 success = self.check_topic() if not success and self.wait_for_first_message: - raise ValueError("Invalid topic or type specified.") + raise ActionError("Invalid topic or type specified.", action=self) elif not success and not self.wait_for_first_message: - raise ValueError("Topic type must be specified. Please provide a valid topic type.") + raise ActionError("Topic type must be specified. Please provide a valid topic type.", action=self) def execute(self, topic_name: str, topic_type: str, latency: float, comparison_operator: bool, rolling_average_count: int, wait_for_first_message: bool): if self.timer != 0: - raise ValueError("Action does not yet support to get retriggered") + raise ActionError("Action does not yet support to get retriggered", action=self) self.timer = time.time() def update(self) -> py_trees.common.Status: diff --git a/scenario_execution_ros/scenario_execution_ros/actions/odometry_distance_traveled.py b/scenario_execution_ros/scenario_execution_ros/actions/odometry_distance_traveled.py index d821e067..e2fa6b52 100644 --- a/scenario_execution_ros/scenario_execution_ros/actions/odometry_distance_traveled.py +++ b/scenario_execution_ros/scenario_execution_ros/actions/odometry_distance_traveled.py @@ -19,7 +19,7 @@ from nav_msgs.msg import Odometry from py_trees.common import Status import py_trees -from scenario_execution.actions.base_action import BaseAction +from scenario_execution.actions.base_action import BaseAction, ActionError class OdometryDistanceTraveled(BaseAction): @@ -49,7 +49,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.callback_group = rclpy.callback_groups.MutuallyExclusiveCallbackGroup() namespace = self.namespace if self.namespace_override: @@ -59,7 +59,7 @@ def setup(self, **kwargs): def execute(self, associated_actor, distance: float, namespace_override: str): if self.namespace != associated_actor["namespace"] or self.namespace_override != namespace_override: - raise ValueError("Runtime change of namespace not supported.") + raise ActionError("Runtime change of namespace not supported.", action=self) self.distance_expected = distance self.distance_traveled = 0.0 self.previous_x = 0 diff --git a/scenario_execution_ros/scenario_execution_ros/actions/ros_action_call.py b/scenario_execution_ros/scenario_execution_ros/actions/ros_action_call.py index b906f537..d8dcd8e6 100644 --- a/scenario_execution_ros/scenario_execution_ros/actions/ros_action_call.py +++ b/scenario_execution_ros/scenario_execution_ros/actions/ros_action_call.py @@ -24,7 +24,7 @@ from rosidl_runtime_py.set_message import set_message_fields import py_trees # pylint: disable=import-error from action_msgs.msg import GoalStatus -from scenario_execution.actions.base_action import BaseAction +from scenario_execution.actions.base_action import BaseAction, ActionError class ActionCallActionState(Enum): @@ -69,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 datatype_in_list = self.action_type_string.split(".") try: @@ -77,7 +77,7 @@ def setup(self, **kwargs): importlib.import_module(".".join(datatype_in_list[0:-1])), datatype_in_list[-1]) except ValueError as e: - raise ValueError(f"Invalid action_type '{self.action_type}':") from e + raise ActionError(f"Invalid action_type '{self.action_type}': {e}", action=self) from e client_kwargs = { "callback_group": self.cb_group, @@ -92,7 +92,7 @@ def setup(self, **kwargs): def execute(self, action_name: str, action_type: str, data: str, transient_local: bool = False): if self.action_name != action_name or self.action_type_string != action_type or self.transient_local != transient_local: - raise ValueError(f"Updating action_name or action_type_string not supported.") + raise ActionError(f"Updating action_name or action_type_string not supported.", action=self) self.parse_data(data) self.current_state = ActionCallActionState.IDLE @@ -103,7 +103,7 @@ def parse_data(self, data): trimmed_data = data.encode('utf-8').decode('unicode_escape') self.data = literal_eval(trimmed_data) except Exception as e: # pylint: disable=broad-except - raise ValueError(f"Error while parsing sevice call data:") from e + raise ActionError(f"Error while parsing sevice call data:", action=self) from e def update(self) -> py_trees.common.Status: """ diff --git a/scenario_execution_ros/scenario_execution_ros/actions/ros_bag_record.py b/scenario_execution_ros/scenario_execution_ros/actions/ros_bag_record.py index a40ae23b..f10659db 100644 --- a/scenario_execution_ros/scenario_execution_ros/actions/ros_bag_record.py +++ b/scenario_execution_ros/scenario_execution_ros/actions/ros_bag_record.py @@ -19,6 +19,7 @@ from enum import Enum import py_trees +from scenario_execution.actions.base_action import ActionError from scenario_execution.actions.run_process import RunProcess import shutil import signal @@ -50,12 +51,11 @@ def setup(self, **kwargs): set up """ if "output_dir" not in kwargs: - raise ValueError("output_dir not defined.") + raise ActionError("output_dir not defined.", 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, topics: list, timestamp_suffix: bool, hidden_topics: bool, storage: str, use_sim_time: bool): # pylint: disable=arguments-differ diff --git a/scenario_execution_ros/scenario_execution_ros/actions/ros_launch.py b/scenario_execution_ros/scenario_execution_ros/actions/ros_launch.py index d546d691..bfb72396 100644 --- a/scenario_execution_ros/scenario_execution_ros/actions/ros_launch.py +++ b/scenario_execution_ros/scenario_execution_ros/actions/ros_launch.py @@ -14,6 +14,7 @@ # # SPDX-License-Identifier: Apache-2.0 +from scenario_execution.actions.base_action import ActionError from scenario_execution.actions.run_process import RunProcess import signal @@ -29,7 +30,7 @@ def execute(self, package_name: str, launch_file: str, arguments: list, wait_for if isinstance(arguments, list): for arg in arguments: if 'key' not in arg or 'value' not in arg: - raise ValueError(f'Invalid argument: {arg}') + raise ActionError(f'Invalid argument: {arg}', action=self) if arg["key"] is not None: self.command.append(f'{arg["key"]}:={arg["value"]}') self.logger.info(f'Command: {" ".join(self.command)}') diff --git a/scenario_execution_ros/scenario_execution_ros/actions/ros_log_check.py b/scenario_execution_ros/scenario_execution_ros/actions/ros_log_check.py index 617b6a16..dc557f92 100644 --- a/scenario_execution_ros/scenario_execution_ros/actions/ros_log_check.py +++ b/scenario_execution_ros/scenario_execution_ros/actions/ros_log_check.py @@ -19,7 +19,7 @@ import rclpy from rcl_interfaces.msg import Log from py_trees.common import Status -from scenario_execution.actions.base_action import BaseAction +from scenario_execution.actions.base_action import BaseAction, ActionError class RosLogCheck(BaseAction): @@ -48,7 +48,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 topic_qos = QoSProfile( depth=100, diff --git a/scenario_execution_ros/scenario_execution_ros/actions/ros_service_call.py b/scenario_execution_ros/scenario_execution_ros/actions/ros_service_call.py index 7a3b5af8..0f84a27d 100644 --- a/scenario_execution_ros/scenario_execution_ros/actions/ros_service_call.py +++ b/scenario_execution_ros/scenario_execution_ros/actions/ros_service_call.py @@ -22,7 +22,7 @@ from rclpy.qos import QoSProfile, DurabilityPolicy from rosidl_runtime_py.set_message import set_message_fields import py_trees # pylint: disable=import-error -from scenario_execution.actions.base_action import BaseAction +from scenario_execution.actions.base_action import BaseAction, ActionError class ServiceCallActionState(Enum): @@ -68,7 +68,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 datatype_in_list = self.service_type_str.split(".") try: @@ -76,7 +76,7 @@ def setup(self, **kwargs): importlib.import_module(".".join(datatype_in_list[0:-1])), datatype_in_list[-1]) except ValueError as e: - raise ValueError(f"Invalid service_type '{self.service_type}':") from e + raise ActionError(f"Invalid service_type '{self.service_type}': {e}", action=self) from e client_kwargs = { "callback_group": self.cb_group, @@ -95,7 +95,7 @@ def setup(self, **kwargs): def execute(self, service_name: str, service_type: str, data: str, transient_local: bool): if self.service_name != service_name or self.service_type_str != service_type or self.data_str != data or self.transient_local != transient_local: - raise ValueError("service_name, service_type and data arguments are not changeable during runtime.") + raise ActionError("service_name, service_type and data arguments are not changeable during runtime.", action=self) self.current_state = ServiceCallActionState.IDLE def update(self) -> py_trees.common.Status: diff --git a/scenario_execution_ros/scenario_execution_ros/actions/ros_set_node_parameter.py b/scenario_execution_ros/scenario_execution_ros/actions/ros_set_node_parameter.py index 3464ad7c..dfda1c66 100644 --- a/scenario_execution_ros/scenario_execution_ros/actions/ros_set_node_parameter.py +++ b/scenario_execution_ros/scenario_execution_ros/actions/ros_set_node_parameter.py @@ -18,6 +18,7 @@ from .ros_service_call import RosServiceCall from rcl_interfaces.msg import ParameterType +from scenario_execution.actions.base_action import ActionError class RosSetNodeParameter(RosServiceCall): @@ -70,7 +71,7 @@ def __init__(self, node_name: str, parameter_name: str, parameter_value: str): def execute(self, node_name: str, parameter_name: str, parameter_value: str): # pylint: disable=arguments-differ,arguments-renamed if self.node_name != node_name or self.parameter_name != parameter_name or self.parameter_value != parameter_value: - raise ValueError("node_name, parameter_name and parameter_value are not changeable during runtime.") + raise ActionError("node_name, parameter_name and parameter_value are not changeable during runtime.", action=self) @staticmethod def is_float(element: any) -> bool: diff --git a/scenario_execution_ros/scenario_execution_ros/actions/ros_topic_check_data.py b/scenario_execution_ros/scenario_execution_ros/actions/ros_topic_check_data.py index d1c1255c..74fa9e84 100644 --- a/scenario_execution_ros/scenario_execution_ros/actions/ros_topic_check_data.py +++ b/scenario_execution_ros/scenario_execution_ros/actions/ros_topic_check_data.py @@ -21,7 +21,7 @@ from rosidl_runtime_py.set_message import set_message_fields from scenario_execution_ros.actions.conversions import get_qos_preset_profile, get_comparison_operator, get_ros_message_type import builtins -from scenario_execution.actions.base_action import BaseAction +from scenario_execution.actions.base_action import BaseAction, ActionError class RosTopicCheckData(BaseAction): @@ -61,7 +61,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.subscriber = self.node.create_subscription( msg_type=get_ros_message_type(self.topic_type), @@ -83,7 +83,7 @@ def execute(self, fail_if_bad_comparison: bool, wait_for_first_message: bool): if self.topic_name != topic_name or self.topic_type != topic_type or self.qos_profile != qos_profile: - raise ValueError("Updating topic parameters not supported.") + raise ActionError("Updating topic parameters not supported.", action=self) self.member_name = member_name self.set_expected_value(expected_value) self.comparison_operator = get_comparison_operator(comparison_operator) @@ -130,7 +130,7 @@ def check_data(self, msg): def set_expected_value(self, expected_value_string): if not isinstance(expected_value_string, str): - raise ValueError("Only string allowed as expected_value.") + raise ActionError("Only string allowed as expected_value.", action=self) error_string = "" try: parsed_value = literal_eval("".join(expected_value_string.split('\\'))) @@ -146,4 +146,4 @@ def set_expected_value(self, expected_value_string): else: set_message_fields(self.expected_value, parsed_value) except TypeError as e: - raise ValueError(f"Could not parse '{expected_value_string}'. {error_string}") from e + raise ActionError(f"Could not parse '{expected_value_string}'. {error_string}", action=self) from e diff --git a/scenario_execution_ros/scenario_execution_ros/actions/ros_topic_monitor.py b/scenario_execution_ros/scenario_execution_ros/actions/ros_topic_monitor.py index 1279c0d5..714baa29 100644 --- a/scenario_execution_ros/scenario_execution_ros/actions/ros_topic_monitor.py +++ b/scenario_execution_ros/scenario_execution_ros/actions/ros_topic_monitor.py @@ -15,7 +15,7 @@ # SPDX-License-Identifier: Apache-2.0 from scenario_execution_ros.actions.conversions import get_qos_preset_profile, get_ros_message_type -from scenario_execution.actions.base_action import BaseAction +from scenario_execution.actions.base_action import BaseAction, ActionError from scenario_execution.model.types import VariableReference import rclpy import py_trees @@ -43,7 +43,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 msg_type = get_ros_message_type(self.topic_type) @@ -61,9 +61,9 @@ def setup(self, **kwargs): def execute(self, topic_name: str, topic_type: str, member_name: str, target_variable: object, qos_profile: tuple): if self.topic_name != topic_name or self.topic_type != topic_type or self.qos_profile != qos_profile: - raise ValueError("Updating topic parameters not supported.") + raise ActionError("Updating topic parameters not supported.", action=self) if not isinstance(target_variable, VariableReference): - raise ValueError(f"'target_variable' is expected to be a variable reference.") + raise ActionError(f"'target_variable' is expected to be a variable reference.", action=self) self.target_variable = target_variable self.member_name = member_name @@ -80,6 +80,6 @@ def get_value(self, msg): try: return check_attr(msg) except AttributeError as e: - raise ValueError(f"invalid member_name '{self.member_name}'") from e + raise ActionError(f"invalid member_name '{self.member_name}'", action=self) from e else: return msg diff --git a/scenario_execution_ros/scenario_execution_ros/actions/ros_topic_publish.py b/scenario_execution_ros/scenario_execution_ros/actions/ros_topic_publish.py index 8d4baf8c..def6cad2 100644 --- a/scenario_execution_ros/scenario_execution_ros/actions/ros_topic_publish.py +++ b/scenario_execution_ros/scenario_execution_ros/actions/ros_topic_publish.py @@ -23,7 +23,7 @@ from py_trees.common import Status from scenario_execution_ros.actions.conversions import get_qos_preset_profile, get_ros_message_type -from scenario_execution.actions.base_action import BaseAction +from scenario_execution.actions.base_action import BaseAction, ActionError class RosTopicPublish(BaseAction): @@ -31,7 +31,7 @@ class RosTopicPublish(BaseAction): class for publish a message on a ROS topic """ - def __init__(self, topic_type: str, topic_name: str, value: str, qos_profile: tuple): + def __init__(self, topic_type: str, topic_name: str, qos_profile: tuple): super().__init__() self.qos_profile = qos_profile self.topic_type = topic_type @@ -48,24 +48,27 @@ 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 - topic_type = get_ros_message_type(self.topic_type) - self.msg_to_pub = topic_type() - self.publisher = self.node.create_publisher( - msg_type=topic_type, - topic=self.topic_name, - qos_profile=get_qos_preset_profile(self.qos_profile) - ) + try: + topic_type = get_ros_message_type(self.topic_type) + self.msg_to_pub = topic_type() + self.publisher = self.node.create_publisher( + msg_type=topic_type, + topic=self.topic_name, + qos_profile=get_qos_preset_profile(self.qos_profile) + ) + except ValueError as e: + raise ActionError(f"{e}", action=self) from e def execute(self, topic_type: str, topic_name: str, value: str, qos_profile: tuple): if self.topic_name != topic_name or self.topic_type != topic_type or self.qos_profile != qos_profile: - raise ValueError("Updating topic parameters not supported.") + raise ActionError("Updating topic parameters not supported.", action=self) if isinstance(value, str): parsed_value = literal_eval("".join(value.split('\\'))) if not isinstance(parsed_value, dict): - raise TypeError(f'Parsed value needs type "dict", got {type(parsed_value)}.') + raise ActionError(f'Parsed value needs type "dict", got {type(parsed_value)}.', action=self) set_message_fields(self.msg_to_pub, parsed_value) elif isinstance(value, dict): set_message_fields(self.msg_to_pub, value) diff --git a/scenario_execution_ros/scenario_execution_ros/actions/ros_topic_wait_for_data.py b/scenario_execution_ros/scenario_execution_ros/actions/ros_topic_wait_for_data.py index f45cadc9..e0bec33f 100644 --- a/scenario_execution_ros/scenario_execution_ros/actions/ros_topic_wait_for_data.py +++ b/scenario_execution_ros/scenario_execution_ros/actions/ros_topic_wait_for_data.py @@ -15,7 +15,7 @@ # SPDX-License-Identifier: Apache-2.0 from scenario_execution_ros.actions.conversions import get_qos_preset_profile, get_ros_message_type -from scenario_execution.actions.base_action import BaseAction +from scenario_execution.actions.base_action import BaseAction, ActionError import rclpy import py_trees @@ -48,7 +48,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.subscriber = self.node.create_subscription( msg_type=get_ros_message_type(self.topic_type), @@ -61,7 +61,7 @@ def setup(self, **kwargs): def execute(self, topic_name, topic_type, qos_profile): if self.topic_name != topic_name or self.topic_type != topic_type or self.qos_profile != qos_profile: - raise ValueError("Updating topic parameters not supported.") + raise ActionError("Updating topic parameters not supported.", action=self) self.found = False def update(self) -> py_trees.common.Status: diff --git a/scenario_execution_ros/scenario_execution_ros/actions/ros_topic_wait_for_topics.py b/scenario_execution_ros/scenario_execution_ros/actions/ros_topic_wait_for_topics.py index 58202f5e..3f67a134 100644 --- a/scenario_execution_ros/scenario_execution_ros/actions/ros_topic_wait_for_topics.py +++ b/scenario_execution_ros/scenario_execution_ros/actions/ros_topic_wait_for_topics.py @@ -16,7 +16,7 @@ import py_trees from rclpy.node import Node -from scenario_execution.actions.base_action import BaseAction +from scenario_execution.actions.base_action import BaseAction, ActionError class RosTopicWaitForTopics(BaseAction): @@ -41,7 +41,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 def update(self) -> py_trees.common.Status: """ diff --git a/scenario_execution_ros/scenario_execution_ros/actions/tf_close_to.py b/scenario_execution_ros/scenario_execution_ros/actions/tf_close_to.py index ae81c385..7eb668e5 100644 --- a/scenario_execution_ros/scenario_execution_ros/actions/tf_close_to.py +++ b/scenario_execution_ros/scenario_execution_ros/actions/tf_close_to.py @@ -27,7 +27,7 @@ from tf2_ros import TransformException # pylint: disable= no-name-in-module from .common import NamespacedTransformListener -from scenario_execution.actions.base_action import BaseAction +from scenario_execution.actions.base_action import BaseAction, ActionError class TfCloseTo(BaseAction): @@ -79,7 +79,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 try: self.marker_handler = kwargs['marker_handler'] @@ -87,7 +87,7 @@ def setup(self, **kwargs): error_message = "didn't find 'marker_handler' 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.feedback_message = f"Waiting for transform map --> base_link" # pylint: disable= attribute-defined-outside-init self.tf_buffer = Buffer() tf_prefix = self.namespace diff --git a/scenario_execution_ros/scenario_execution_ros/external_methods/__init__.py b/scenario_execution_ros/scenario_execution_ros/external_methods/__init__.py new file mode 100644 index 00000000..3ba13780 --- /dev/null +++ b/scenario_execution_ros/scenario_execution_ros/external_methods/__init__.py @@ -0,0 +1,15 @@ +# 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 diff --git a/scenario_execution_ros/scenario_execution_ros/external_methods/msg_conversion.py b/scenario_execution_ros/scenario_execution_ros/external_methods/msg_conversion.py new file mode 100644 index 00000000..1cbb9f17 --- /dev/null +++ b/scenario_execution_ros/scenario_execution_ros/external_methods/msg_conversion.py @@ -0,0 +1,50 @@ +# 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 geometry_msgs.msg import PoseWithCovarianceStamped +import json +from rosidl_runtime_py.convert import message_to_ordereddict + +from transforms3d.euler import quat2euler + + +def get_object_member(in_value, member_name=""): + target = in_value + if member_name: + splitted_member = member_name.split('.') + for mem in splitted_member: + if not hasattr(target, mem): + raise ValueError(f"Member '{mem}' not found in '{target}") + target = getattr(target, mem) + return target + + +def to_dict(in_value, member_name=""): + target = get_object_member(in_value, member_name) + return json.loads(json.dumps(message_to_ordereddict(target))) + + +def to_pose_3d(in_value): + if isinstance(in_value, PoseWithCovarianceStamped): + pose3d = {} + pose3d["position"] = to_dict(in_value.pose.pose.position) + roll, pitch, yaw = quat2euler([in_value.pose.pose.orientation.w, in_value.pose.pose.orientation.x, + in_value.pose.pose.orientation.y, in_value.pose.pose.orientation.z]) + pose3d["orientation"] = {'roll': roll, 'pitch': pitch, 'yaw': yaw} + return pose3d + elif isinstance(in_value, dict) and in_value == {}: # allow empty input to enable expression evaluation + return in_value + else: + raise ValueError(f"to_pose3d not implemented for type {type(in_value)}") diff --git a/scenario_execution_ros/scenario_execution_ros/lib_osc/ros.osc b/scenario_execution_ros/scenario_execution_ros/lib_osc/ros.osc index e74b003b..18ae923d 100644 --- a/scenario_execution_ros/scenario_execution_ros/lib_osc/ros.osc +++ b/scenario_execution_ros/scenario_execution_ros/lib_osc/ros.osc @@ -26,7 +26,11 @@ enum lifecycle_state: [ actor differential_drive_robot inherits robot: namespace: string = '' - + +struct msg_conversion: + def get_object_member(in_value: string, member_name: string = "") -> string is external scenario_execution_ros.external_methods.msg_conversion.get_object_member() + def to_pose_3d(in_value: string) -> pose_3d is external scenario_execution_ros.external_methods.msg_conversion.to_pose_3d() + action action_call: # Call a ros action and wait for the result action_name: string # name of the action to connect to diff --git a/scenario_execution_ros/test/test_external_methods_msg_conversion.py b/scenario_execution_ros/test/test_external_methods_msg_conversion.py new file mode 100644 index 00000000..58fe1728 --- /dev/null +++ b/scenario_execution_ros/test/test_external_methods_msg_conversion.py @@ -0,0 +1,102 @@ +# 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 unittest +import rclpy +import py_trees +from scenario_execution_ros import ROSScenarioExecution +from scenario_execution.model.osc2_parser import OpenScenario2Parser +from scenario_execution.model.model_to_py_tree import create_py_tree +from scenario_execution.model.model_blackboard import create_py_tree_blackboard +from scenario_execution.utils.logging import Logger +from antlr4.InputStream import InputStream + +os.environ["PYTHONUNBUFFERED"] = '1' + + +class TestExternalMethodsMsgConversion(unittest.TestCase): + # pylint: disable=missing-function-docstring + + def setUp(self): + rclpy.init() + self.parser = OpenScenario2Parser(Logger('test', False)) + self.scenario_execution_ros = ROSScenarioExecution() + self.tree = py_trees.composites.Sequence(name="", memory=True) + + def execute(self, scenario_content): + parsed_tree = self.parser.parse_input_stream(InputStream(scenario_content)) + model = self.parser.create_internal_model(parsed_tree, self.tree, "test.osc", False) + create_py_tree_blackboard(model, self.tree, self.parser.logger, False) + self.tree = create_py_tree(model, self.tree, self.parser.logger, False) + self.scenario_execution_ros.tree = self.tree + self.scenario_execution_ros.run() + + def tearDown(self): + rclpy.try_shutdown() + + def test_get_object_member(self): + scenario_content = """ +import osc.ros +import osc.helpers + +struct current_state: + var test_pose: string + +scenario test_success: + timeout(10s) + current: current_state + + do parallel: + topic_monitor('/test_pose', 'geometry_msgs.msg.PoseWithCovarianceStamped', current.test_pose) + serial: + wait elapsed(0.5s) + topic_publish('/test_pose', 'geometry_msgs.msg.PoseWithCovarianceStamped', "{ \\\'pose\\\': { \\\'pose\\\': { \\\'position\\\': { \\\'x\\\': 42 }}}}") + serial: + wait elapsed(3s) + topic_publish('/pose_only', 'geometry_msgs.msg.Pose', msg_conversion.get_object_member(current.test_pose, "pose.pose")) + + serial: + check_data('/pose_only', 'geometry_msgs.msg.Pose', expected_value: '{ \\\"position\\\": { \\\"x\\\": 42 }}') + emit end +""" + self.execute(scenario_content) + self.assertTrue(self.scenario_execution_ros.process_results()) + + def test_to_pose_3d(self): + scenario_content = """ +import osc.ros +import osc.helpers + +struct current_state: + var test_pose: string + +scenario test_success: + timeout(10s) + current: current_state + test_pose : pose_3d = pose_3d(position_3d(x: 42)) + do parallel: + topic_monitor('/test_pose', 'geometry_msgs.msg.PoseWithCovarianceStamped', current.test_pose) + serial: + wait elapsed(0.5s) + topic_publish('/test_pose', 'geometry_msgs.msg.PoseWithCovarianceStamped', "{ \\\'pose\\\': { \\\'pose\\\': { \\\'position\\\': { \\\'x\\\': 42 }, \\\'orientation\\\': { \\\'w\\\': 1 }}}}") + serial: + wait test_pose == msg_conversion.to_pose_3d(current.test_pose) + emit end +""" + self.execute(scenario_content) + self.assertTrue(self.scenario_execution_ros.process_results())