Merge branch 'main' into moveit_generic

Author: fred-labs

docs/libraries.rst

@@ -285,6 +285,47 @@ Be depressed, always fail.
 The tickling never ends...
 
 
+
+``decrement()``
+^^^^^^^^^^^^^^^
+
+Decrement the value of a variable.
+
+.. list-table:: 
+   :widths: 15 15 5 65
+   :header-rows: 1
+   :class: tight-table   
+   
+   * - Parameter
+     - Type
+     - Default
+     - Description
+   * - ``target_variable``
+     - ``variable``
+     -
+     - Variable to decrement
+
+
+``increment()``
+^^^^^^^^^^^^^^^
+
+Increment the value of a variable.
+
+.. list-table:: 
+   :widths: 15 15 5 65
+   :header-rows: 1
+   :class: tight-table   
+   
+   * - Parameter
+     - Type
+     - Default
+     - Description
+   * - ``target_variable``
+     - ``variable``
+     -
+     - Variable to increment
+
+
 ``log()``
 ^^^^^^^^^
 
@@ -924,6 +965,10 @@ Play back a ROS bag.
      - ``float``
      - ``1.0``
      - if ``publish_clock`` is true, publish to ``/clock`` at the specified frequency in Hz, to act as a ROS Time Source.
+   * - ``start_offset``
+     - ``float``
+     - ``0.0``
+     - start the playback this many seconds into the bag file
 
 
 ``bag_record()``

scenario_execution/scenario_execution/actions/base_action.py

@@ -15,7 +15,6 @@
 # SPDX-License-Identifier: Apache-2.0
 
 import py_trees
-from scenario_execution.model.types import ParameterDeclaration, ScenarioDeclaration
 from scenario_execution.model.error import OSC2Error
 import inspect
 
@@ -55,8 +54,11 @@ def initialise(self):
             if self.resolve_variable_reference_arguments_in_execute:
                 final_args = self._model.get_resolved_value(self.get_blackboard_client(), skip_keys=self.execute_skip_args)
             else:
-                final_args = self._model.get_resolved_value_with_variable_references(
-                    self.get_blackboard_client(), skip_keys=self.execute_skip_args)
+                try:
+                    final_args = self._model.get_resolved_value_with_variable_references(
+                        self.get_blackboard_client(), skip_keys=self.execute_skip_args)
+                except ValueError as e:
+                    raise ActionError(f"Error initializing action: {e}", action=self) from e
 
             if self._model.actor:
                 final_args["associated_actor"] = self._model.actor.get_resolved_value(self.get_blackboard_client())
@@ -71,24 +73,14 @@ def _set_base_properities(self, name, model, logger):
     def get_blackboard_client(self):
         if self.blackboard:
             return self.blackboard
-
-        def get_blackboard_namespace(node: ParameterDeclaration):
-            parent = node.get_parent()
-            while parent is not None and not isinstance(parent, ScenarioDeclaration):
-                parent = parent.get_parent()
-            if parent:
-                return parent.name
-            else:
-                return None
-
-        self.blackboard = self.attach_blackboard_client(name=self.name, namespace=get_blackboard_namespace(self._model))
+        self.blackboard = self.attach_blackboard_client(name=self.name)
         return self.blackboard
 
     def register_access_to_associated_actor_variable(self, variable_name):
         if not self._model.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 = self._model.actor.get_qualified_name()
         model_blackboard_name += "/" + variable_name
         blackboard.register_key(model_blackboard_name, access=py_trees.common.Access.WRITE)
         return model_blackboard_name

scenario_execution/scenario_execution/actions/decrement.py

@@ -0,0 +1,39 @@
+# 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 py_trees  # pylint: disable=import-error
+from scenario_execution.actions.base_action import BaseAction, ActionError
+from scenario_execution.model.types import VariableReference
+
+
+class Decrement(BaseAction):
+    """
+    Class to decrement the value of a variable
+    """
+
+    def __init__(self):
+        super().__init__(resolve_variable_reference_arguments_in_execute=False)
+        self.target_variable = None
+
+    def execute(self, target_variable: object):
+        if not isinstance(target_variable, VariableReference):
+            raise ActionError(
+                f"'target_variable' is expected to be a variable reference but is {type(target_variable).__name__}.", action=self)
+        self.target_variable = target_variable
+
+    def update(self) -> py_trees.common.Status:
+        self.target_variable.set_value(self.target_variable.get_value() - 1)
+        return py_trees.common.Status.SUCCESS

scenario_execution/scenario_execution/actions/increment.py

@@ -0,0 +1,39 @@
+# 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 py_trees  # pylint: disable=import-error
+from scenario_execution.actions.base_action import BaseAction, ActionError
+from scenario_execution.model.types import VariableReference
+
+
+class Increment(BaseAction):
+    """
+    Class to increment the value of a variable
+    """
+
+    def __init__(self):
+        super().__init__(resolve_variable_reference_arguments_in_execute=False)
+        self.target_variable = None
+
+    def execute(self, target_variable: object):
+        if not isinstance(target_variable, VariableReference):
+            raise ActionError(
+                f"'target_variable' is expected to be a variable reference but is {type(target_variable).__name__}.", action=self)
+        self.target_variable = target_variable
+
+    def update(self) -> py_trees.common.Status:
+        self.target_variable.set_value(self.target_variable.get_value() + 1)
+        return py_trees.common.Status.SUCCESS

scenario_execution/scenario_execution/lib_osc/helpers.osc

@@ -9,6 +9,14 @@ enum signal: [
     sigterm = 15
 ]
 
+action increment:
+    # Increment the value of a variable
+    target_variable: string              # variable to increment
+
+action decrement:
+    # Decrement the value of a variable
+    target_variable: string              # variable to decrement
+
 action log:
     # Print out a message
     msg: string                          # Message to print

scenario_execution/scenario_execution/model/model_blackboard.py

@@ -16,7 +16,7 @@
 
 import py_trees
 
-from scenario_execution.model.types import ParameterDeclaration, StructuredDeclaration, VariableDeclaration
+from scenario_execution.model.types import ParameterDeclaration, StructuredDeclaration, VariableDeclaration, ScenarioDeclaration, Declaration
 from scenario_execution.model.model_base_visitor import ModelBaseVisitor
 from scenario_execution.model.error import OSC2ParsingError
 
@@ -47,12 +47,32 @@ def __init__(self, logger, tree) -> None:
             self.blackboard = tree.attach_blackboard_client(name="ModelToPyTree")
 
         def visit_parameter_declaration(self, node: ParameterDeclaration):
-            super().visit_parameter_declaration(node)
             parameter_type = node.get_type()[0]
-            if isinstance(parameter_type, StructuredDeclaration):
-                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)
-                    setattr(self.blackboard, blackboard_var_name, variable_dec.get_resolved_value())
+            if isinstance(parameter_type, StructuredDeclaration) and self.needs_blackboard_entry(node):
+                self.create_blackboard_entries(parameter_type, node.get_qualified_name())
+
+        def create_blackboard_entries(self, elem, prefix):
+            for variable_dec in elem.find_children_of_type(VariableDeclaration):
+                fqn = prefix + "/" + variable_dec.name
+                self.blackboard.register_key(fqn, access=py_trees.common.Access.WRITE)
+                setattr(self.blackboard, fqn, variable_dec.get_resolved_value())
+
+            for child in elem.find_children_of_type(ParameterDeclaration):
+                child_type = child.get_type()[0]
+                if isinstance(child_type, Declaration):
+                    self.create_blackboard_entries(child_type, prefix + "/" + child.name)
+
+        def needs_blackboard_entry(self, node):
+            current = node.get_parent()
+            while current:
+                if isinstance(current, ScenarioDeclaration):
+                    return True
+                current = current.get_parent()
+            return False
+
+        def visit_variable_declaration(self, node: VariableDeclaration):
+            if self.needs_blackboard_entry(node):
+                blackboard_var_name = node.get_fully_qualified_var_name()
+                self.blackboard.register_key(blackboard_var_name, access=py_trees.common.Access.WRITE)
+                setattr(self.blackboard, blackboard_var_name, node.get_resolved_value())

scenario_execution/scenario_execution/model/model_to_py_tree.py

@@ -104,7 +104,7 @@ def update(self):
 class ExpressionBehavior(BaseAction):  # py_trees.behaviour.Behaviour):
 
     def __init__(self, name: "ExpressionBehavior", expression: Expression, model, logger):
-        super().__init__()
+        super().__init__(resolve_variable_reference_arguments_in_execute=False)
         self._set_base_properities(name, model, logger)
         self.expression = expression
 
@@ -146,8 +146,7 @@ def visit_scenario_declaration(self, node: ScenarioDeclaration):
             self.__cur_behavior.name = scenario_name
 
             self.blackboard = self.__cur_behavior.attach_blackboard_client(
-                name="ModelToPyTree",
-                namespace=scenario_name)
+                name="ModelToPyTree")
 
             super().visit_scenario_declaration(node)
 

scenario_execution/scenario_execution/model/types.py

@@ -968,16 +968,6 @@ def accept(self, visitor):
         else:
             return visitor.visit_children(self)
 
-    def get_fully_qualified_var_name(self, include_scenario):
-        name = self.name
-        parent = self.get_parent()
-        while parent and not isinstance(parent, ScenarioDeclaration):
-            name = parent.name + "/" + name
-            parent = parent.get_parent()
-        if include_scenario and parent and parent.name:
-            name = parent.name + "/" + name
-        return name
-
 
 class ParameterReference(ModelElement):
 
@@ -1277,6 +1267,14 @@ def accept(self, visitor):
         else:
             return visitor.visit_children(self)
 
+    def get_fully_qualified_var_name(self):
+        name = self.name
+        parent = self.get_parent()
+        while parent:
+            name = parent.name + "/" + name
+            parent = parent.get_parent()
+        return name
+
 
 class KeepConstraintDeclaration(Declaration):
 
@@ -2245,20 +2243,29 @@ def get_type_string(self):
             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")
-        if not isinstance(self.ref[0], ParameterDeclaration):
-            raise ValueError("Variable Reference only supported if reference is part of a parameter declaration")
-        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
+        if isinstance(self.ref, list):
+            if len(self.ref) == 0:
+                raise ValueError("Variable Reference only supported if reference is list with at least one element")
+            if not isinstance(self.ref[0], ParameterDeclaration):
+                raise ValueError("Variable Reference only supported if reference is part of a parameter declaration")
+            fqn = self.ref[0].get_qualified_name()
+            var_found = False
+            for elem in self.ref[1:]:
+                if isinstance(elem, VariableDeclaration):
+                    var_found = True
+                fqn += "/" + elem.name
+            if not var_found:
+                raise ValueError(f"No variable found in '{fqn}.")
+        elif isinstance(self.ref, VariableDeclaration):
+            fqn = self.ref.get_fully_qualified_var_name()
+        else:
+            raise ValueError(f"Only references to VariableDeclaration supported, not '{type(self.ref).__name__}'.")
+
         blackboard.register_key(fqn, access=py_trees.common.Access.WRITE)
         return VariableReference(blackboard, fqn)
 
     def get_variable_reference(self, blackboard):
-        if isinstance(self.ref, list) and any(isinstance(x, VariableDeclaration) for x in self.ref):
+        if (isinstance(self.ref, list) and any(isinstance(x, VariableDeclaration) for x in self.ref)) or isinstance(self.ref, VariableDeclaration):
             return self.get_blackboard_reference(blackboard)
         else:
             return None

scenario_execution/setup.py

@@ -70,6 +70,8 @@
             'scenario_execution = scenario_execution.scenario_execution_base:main',
         ],
         'scenario_execution.actions': [
+            'increment = scenario_execution.actions.increment:Increment',
+            'decrement = scenario_execution.actions.decrement:Decrement',
             'log = scenario_execution.actions.log:Log',
             'run_process = scenario_execution.actions.run_process:RunProcess',
         ],