Add topic_monitor action

docs/development.rst

@@ -99,3 +99,6 @@ Implement an Action
   - ``input_dir``: Directory containing the scenario file
   - ``output_dir``: If given on command-line, contains the directory to save output to
   - ``node``: (``scenario_execution_ros`` only): ROS node to utilize (e.g. create subscribers)
+- If your action makes use of variables, set ``resolve_variable_reference_arguments_in_execute`` in ``BaseAction.__init()`` to  ``False``.
+  The ``execute()`` method arguments will then contain resolved values as before, except for variable arguments which are accessible
+  as ``VariableReference`` (with methods ``set_value()`` and ``get_value()``).

docs/libraries.rst

@@ -309,15 +309,25 @@ Set a parameter of a node.
 - ``parameter_name: string``: Name of the parameter
 - ``parameter_value: string``: Value of the parameter
 
+``topic_monitor()``
+"""""""""""""""""""
+
+Subscribe to a topic and store the last message within a variable.
+
+- ``topic_name: string``: name of the topic to monitor
+- ``topic_type: string``: class of the message type (e.g. ``std_msgs.msg.String``)
+- ``target_variable: variable``: variable to store the received value (e.g. a ``var`` within an actor instance)
+- ``qos_profile: qos_preset_profiles``: QoS profile for the subscriber (default: ``qos_preset_profiles!system_default``)
+
 ``topic_publish()``
 """""""""""""""""""
 
 Publish a message on a topic.
 
 - ``topic_name: string``: Name of the topic to publish to
 - ``topic_type: string``: Class of the message type (e.g. ``std_msgs.msg.String``)
+- ``value: string``: Value to publish (can either be a string that gets parsed, a struct or a message object stored within a variable)
 - ``qos_profile: qos_preset_profiles``: QoS Preset Profile for the subscriber (default: ``qos_preset_profiles!system_default``)
-- ``value: string``: Value to publish
 
 ``wait_for_data()``
 """""""""""""""""""
@@ -329,7 +339,6 @@ In the background, this action uses `wait_for_data() <https://py-trees-ros.readt
 - ``topic_name: string``: Name of the topic to connect to
 - ``topic_type: string``: Class of the message type (e.g. ``std_msgs.msg.String``)
 - ``qos_profile: qos_preset_profiles``: QoS Preset Profile for the subscriber (default: ``qos_preset_profiles!system_default``)
-- ``clearing_policy: clearing_policy``: When to clear the data (default: ``clearing_policy!on_initialise``)
 
 
 ``wait_for_topics()``

scenario_execution/scenario_execution/actions/base_action.py

@@ -23,8 +23,9 @@ class BaseAction(py_trees.behaviour.Behaviour):
     # subclasses might implement __init__() with the same arguments as defined in osc
     # CAUTION: __init__() only gets the initial parameter values. In case variables get modified during scenario execution,
     #          the latest values are available in execute() only.
-    def __init__(self):
+    def __init__(self, resolve_variable_reference_arguments_in_execute=True):
         self.blackboard = None
+        self.resolve_variable_reference_arguments_in_execute = resolve_variable_reference_arguments_in_execute
         super().__init__(self.__class__.__name__)
 
     # Subclasses might implement execute() with the same arguments as defined in osc.
@@ -41,7 +42,11 @@ def shutdown(self):
     def initialise(self):
         execute_method = getattr(self, "execute", None)
         if execute_method is not None and callable(execute_method):
-            final_args = self.model.get_resolved_value(self.get_blackboard_client())
+
+            if self.resolve_variable_reference_arguments_in_execute:
+                final_args = self.model.get_resolved_value(self.get_blackboard_client())
+            else:
+                final_args = self.model.get_resolved_value_with_variable_references(self.get_blackboard_client())
 
             if self.model.actor:
                 final_args["associated_actor"] = self.model.actor.get_resolved_value(self.get_blackboard_client())
@@ -70,7 +75,7 @@ def get_blackboard_namespace(node: ParameterDeclaration):
 
     def set_associated_actor_variable(self, variable_name, value):
         if not self.model.actor:
-            raise ValueError("Mddel does not have 'actor'.")
+            raise ValueError("Model does not have 'actor'.")
         blackboard = self.get_blackboard_client()
         model_blackboard_name = self.model.actor.get_fully_qualified_var_name(include_scenario=False)
         model_blackboard_name += "/" + variable_name

scenario_execution/scenario_execution/model/model_blackboard.py

@@ -51,8 +51,8 @@ def visit_parameter_declaration(self, node: ParameterDeclaration):
             super().visit_parameter_declaration(node)
             parameter_type = node.get_type()[0]
             if isinstance(parameter_type, StructuredDeclaration):
-                prefix = node.get_fully_qualified_var_name(include_scenario=True)
                 for variable_dec in parameter_type.find_children_of_type(VariableDeclaration):
+                    prefix = node.get_fully_qualified_var_name(include_scenario=True)
                     blackboard_var_name = prefix + "/" + variable_dec.name
 
                     self.blackboard.register_key(blackboard_var_name, access=py_trees.common.Access.WRITE)

scenario_execution/scenario_execution/model/model_to_py_tree.py

@@ -179,13 +179,13 @@ def compare_method_arguments(self, method, expected_args, behavior_name, node):
                 raise OSC2ParsingError(
                     msg=f'Plugin {behavior_name} {method.__name__} method is missing argument "self".', context=node.get_ctx())
 
-            missing_args = []
-            unknown_args = copy.copy(expected_args)
+            unknown_args = []
+            missing_args = copy.copy(expected_args)
             for element in method_args:
                 if element not in expected_args:
-                    missing_args.append(element)
+                    unknown_args.append(element)
                 else:
-                    unknown_args.remove(element)
+                    missing_args.remove(element)
             error_string = ""
             if missing_args:
                 error_string += "missing: " + ", ".join(missing_args)
@@ -226,24 +226,27 @@ def visit_behavior_invocation(self, node: BehaviorInvocation):
                     context=node.get_ctx()
                 )
 
-            expected_args = node.get_parameter_names()
-            expected_args.append("self")
+            expected_args = ["self"]
             if node.actor:
                 expected_args.append("associated_actor")
+            expected_args += node.get_parameter_names()
 
             # check plugin constructor
             init_method = getattr(behavior_cls, "__init__", None)
             init_args = None
             if init_method is not None:
                 # if __init__() is defined, check parameters. Allowed:
                 # - __init__(self)
-                # - __init__(self, <all-osc-defined-args)
+                # - __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)
-                if init_args != ["self"] and set(init_args) != set(expected_args):
+                if init_args != ["self"] and \
+                        init_args != ["self", "resolve_variable_reference_arguments_in_execute"] and \
+                        set(init_args) != set(expected_args):
                     raise OSC2ParsingError(
-                        msg=f'Plugin {behavior_name}: __init__() either only has "self" argument or all arguments defined in osc{error_string}.', context=node.get_ctx()
+                        msg=f'Plugin {behavior_name}: __init__() either only has "self" argument or all arguments defined in osc. {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)
@@ -259,7 +262,7 @@ def visit_behavior_invocation(self, node: BehaviorInvocation):
             self.logger.debug(
                 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']:
+                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)
 
                     if node.actor:

scenario_execution/scenario_execution/model/types.py

@@ -1504,6 +1504,22 @@ def get_base_type(self):
     def get_type(self):
         return self.behavior, False
 
+    def get_resolved_value_with_variable_references(self, blackboard):
+        params = self.get_resolved_value(blackboard)
+
+        pos = 0
+        param_keys = list(params.keys())
+        for child in self.get_children():
+            if isinstance(child, PositionalArgument):
+                if isinstance(child.get_child(0), IdentifierReference):
+                    params[param_keys[pos]] = child.get_child(0).get_blackboard_reference(blackboard)
+                pos += 1
+            elif isinstance(child, NamedArgument):
+                if isinstance(child.get_child(0), IdentifierReference):
+                    params[child.name] = child.get_child(0).get_blackboard_reference(blackboard)
+
+        return params
+
 
 class ModifierInvocation(ModelElement):
 
@@ -2121,6 +2137,24 @@ def accept(self, visitor):
             return visitor.visit_children(self)
 
 
+class VariableReference(object):
+    # To access variables from within action implementations
+
+    def __init__(self, blackboard, ref) -> None:
+        self.blackboard = blackboard
+        self.ref = ref
+        self.blackboard.register_key(ref, access=py_trees.common.Access.WRITE)
+
+    def __str__(self):
+        return f"VariableReference({self.ref})"
+
+    def set_value(self, val):
+        setattr(self.blackboard, self.ref, val)
+
+    def get_value(self):
+        return getattr(self.blackboard, self.ref)
+
+
 class IdentifierReference(ModelElement):
 
     def __init__(self, ref):
@@ -2153,18 +2187,23 @@ def get_type_string(self):
         else:
             return self.ref.get_type_string()
 
+    def get_blackboard_reference(self, blackboard):
+        if not isinstance(self.ref, list) or len(self.ref) == 0:
+            raise ValueError("Variable Reference only supported if reference is list with at least one element")
+        fqn = self.ref[0].get_fully_qualified_var_name(include_scenario=False)
+        if blackboard is None:
+            raise ValueError("Variable Reference found, but no blackboard client available.")
+        for sub_elem in self.ref[1:]:
+            fqn += "/" + sub_elem.name
+        blackboard.register_key(fqn, access=py_trees.common.Access.WRITE)
+        return VariableReference(blackboard, fqn)
+
     def get_resolved_value(self, blackboard=None):
         if isinstance(self.ref, list):
             ref = self.ref[0]
             if any(isinstance(x, VariableDeclaration) for x in self.ref):
-                fqn = ref.get_fully_qualified_var_name(include_scenario=False)
-                if blackboard is None:
-                    raise ValueError("Variable Reference found, but no blackboard client available.")
-                for sub_elem in self.ref[1:]:
-                    fqn += "/" + sub_elem.name
-
-                blackboard.register_key(fqn, access=py_trees.common.Access.READ)
-                return getattr(blackboard, fqn)
+                var_ref = self.get_blackboard_reference(blackboard)
+                return var_ref.get_value()
             else:
                 val = ref.get_resolved_value(blackboard)
                 for sub_elem in self.ref[1:]:

scenario_execution_ros/scenario_execution_ros/actions/conversions.py

@@ -17,9 +17,21 @@
 """ common conversions """
 
 import operator
+import importlib
 from rclpy.qos import QoSPresetProfiles
 
 
+def get_ros_message_type(message_type_string):
+    if not message_type_string:
+        raise ValueError("Empty message type.")
+
+    datatype_in_list = message_type_string.split(".")
+    try:
+        return getattr(importlib.import_module(".".join(datatype_in_list[0:-1])), datatype_in_list[-1])
+    except (ModuleNotFoundError, ValueError) as e:
+        raise ValueError(f"Could not find message type {message_type_string}: {e}") from e
+
+
 def get_qos_preset_profile(qos_profile):
     """
     Get qos preset for enum value

scenario_execution_ros/scenario_execution_ros/actions/ros_topic_check_data.py

@@ -16,11 +16,10 @@
 
 import py_trees
 import rclpy
-import importlib
 import operator
 from ast import literal_eval
 from rosidl_runtime_py.set_message import set_message_fields
-from scenario_execution_ros.actions.conversions import get_qos_preset_profile, get_comparison_operator
+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
 
@@ -41,11 +40,9 @@ def __init__(self,
                  fail_if_bad_comparison: bool,
                  wait_for_first_message: bool):
         super().__init__()
-        datatype_in_list = topic_type.split(".")
         self.topic_name = topic_name
-        self.topic_type = getattr(
-            importlib.import_module(".".join(datatype_in_list[0:-1])),
-            datatype_in_list[-1])
+
+        self.topic_type = get_ros_message_type(topic_type)
         self.qos_profile = get_qos_preset_profile(qos_profile)
         self.member_name = member_name
 

scenario_execution_ros/scenario_execution_ros/actions/ros_topic_monitor.py

@@ -0,0 +1,67 @@
+# 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 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.model.types import VariableReference
+import rclpy
+import py_trees
+
+
+class RosTopicMonitor(BaseAction):
+
+    def __init__(self, topic_name: str, topic_type: str, target_variable: object, qos_profile: tuple):
+        super().__init__(resolve_variable_reference_arguments_in_execute=False)
+        self.target_variable = None
+        self.topic_type = topic_type
+        self.qos_profile = qos_profile
+        self.topic_name = topic_name
+        self.subscriber = None
+        self.node = None
+
+    def setup(self, **kwargs):
+        """
+        Setup the subscriber
+        """
+        try:
+            self.node: rclpy.Node = kwargs['node']
+        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
+
+        self.subscriber = self.node.create_subscription(
+            msg_type=get_ros_message_type(self.topic_type),
+            topic=self.topic_name,
+            callback=self._callback,
+            qos_profile=get_qos_preset_profile(self.qos_profile),
+            callback_group=rclpy.callback_groups.ReentrantCallbackGroup()
+        )
+        self.feedback_message = f"Monitoring data on {self.topic_name}"  # pylint: disable= attribute-defined-outside-init
+
+    def execute(self, topic_name, topic_type, target_variable, 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.")
+        if not isinstance(target_variable, VariableReference):
+            raise ValueError(f"'target_variable' is expected to be a variable reference.")
+        self.target_variable = target_variable
+
+    def update(self) -> py_trees.common.Status:
+        return py_trees.common.Status.SUCCESS
+
+    def _callback(self, msg):
+        if self.target_variable is not None:
+            self.target_variable.set_value(msg)