Support usage of ros messages as parameters

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

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):
@@ -27,6 +28,7 @@ def __init__(self, resolve_variable_reference_arguments_in_execute=True):
         self._model = None
         self.logger = None
         self.blackboard = None
+        self.osc_ctx = None
         self.resolve_variable_reference_arguments_in_execute = resolve_variable_reference_arguments_in_execute
         super().__init__(self.__class__.__name__)
 
@@ -55,10 +57,11 @@ def initialise(self):
                 final_args["associated_actor"]["name"] = self._model.actor.name
             self.execute(**final_args)  # pylint: disable=no-member
 
-    def _set_base_properities(self, name, model, logger):
+    def _set_base_properities(self, name, model, logger, osc_ctx):
         self.name = name
         self._model = model
         self.logger = logger
+        self.osc_ctx = osc_ctx
 
     def get_blackboard_client(self):
         if self.blackboard:
@@ -94,3 +97,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.osc_ctx
+        super().__init__(msg, ctx, *args)

scenario_execution/scenario_execution/model/error.py

@@ -18,24 +18,33 @@
 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:
+            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}: "
+        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
 
 
@@ -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,16 +336,18 @@ 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()
-                    instance._set_base_properities(action_name, node, self.logger)  # pylint: disable=protected-access
+                    instance._set_base_properities(action_name, node, self.logger, node.get_ctx())  # pylint: disable=protected-access
                 except Exception as e:
                     raise OSC2ParsingError(msg=f'Error while initializing plugin {behavior_name}: {e}', context=node.get_ctx()) from e
                 self.__cur_behavior.add_child(instance)

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 <line: {e.line}, column: {e.column}> 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)

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
 

scenario_execution_ros/scenario_execution_ros/actions/ros_topic_publish.py

@@ -23,15 +23,15 @@
 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):
     """
     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,15 +48,18 @@ 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:

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

scenario_execution_ros/scenario_execution_ros/external_methods/msg_conversion.py

@@ -0,0 +1,48 @@
+# 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_pose3d(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
+    else:
+        raise ValueError(f"to_pose3d not implemented for type {type(in_value)}")

scenario_execution_ros/scenario_execution_ros/lib_osc/ros.osc

@@ -26,7 +26,12 @@ 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_dict(in_value: string, member_name: string = "") -> pose_3d is external scenario_execution_ros.external_methods.msg_conversion.to_dict()
+    def to_pose3d(in_value: string) -> pose_3d is external scenario_execution_ros.external_methods.msg_conversion.to_pose3d()
+
 action action_call:
     # Call a ros action and wait for the result
     action_name: string                  # name of the action to connect to