add action: wait_for_nodes (#164)

Author: fred-labs

docs/libraries.rst

@@ -1227,6 +1227,26 @@ Wait for any message on a ROS topic.
      - QoS Preset Profile for the subscriber (default: ``qos_preset_profiles!system_default``)
 
 
+``wait_for_nodes()``
+^^^^^^^^^^^^^^^^^^^^^
+
+Wait for nodes to get available.
+
+.. list-table:: 
+   :widths: 15 15 5 65
+   :header-rows: 1
+   :class: tight-table   
+   
+   * - Parameter
+     - Type
+     - Default
+     - Description
+   * - ``nodes``
+     - ``list of string``
+     - 
+     - List of nodes to wait for
+
+
 ``wait_for_topics()``
 ^^^^^^^^^^^^^^^^^^^^^
 

scenario_execution_ros/scenario_execution_ros/actions/ros_wait_for_nodes.py

@@ -0,0 +1,51 @@
+# 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
+from rclpy.node import Node
+from scenario_execution.actions.base_action import BaseAction
+from ros2node.api import get_node_names
+
+
+class RosWaitForNodes(BaseAction):
+    """
+    Wait for nodes to get available
+    """
+
+    def __init__(self, nodes: list):
+        super().__init__()
+        if not isinstance(nodes, list):
+            raise TypeError(f'Nodes needs to be list of string, got {type(nodes)}.')
+        else:
+            self.nodes = nodes
+        self.node = None
+
+    def setup(self, **kwargs):
+        try:
+            self.node: 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
+
+    def update(self) -> py_trees.common.Status:
+        available_nodes = get_node_names(node=self.node, include_hidden_nodes=False)
+        available_nodes = [seq[2] for seq in available_nodes]
+        result = all(elem in available_nodes for elem in self.nodes)
+        if result:
+            return py_trees.common.Status.SUCCESS
+        else:
+            return py_trees.common.Status.RUNNING

scenario_execution_ros/scenario_execution_ros/lib_osc/ros.osc

@@ -145,6 +145,10 @@ action wait_for_data:
     topic_type: string                   # class of the message type (e.g. std_msgs.msg.String)
     qos_profile: qos_preset_profiles = qos_preset_profiles!system_default # qos profile for the subscriber
 
+action wait_for_nodes:
+    # Wait for nodes to get available.
+    nodes: list of string                # list of nodes to wait for
+
 action wait_for_topics:
     # Wait for topics to get available (i.e. publisher gets available).
-    topics: list of topics               # list of topics to wait for
+    topics: list of string               # list of topics to wait for

scenario_execution_ros/setup.py

@@ -64,6 +64,7 @@
             'topic_monitor = scenario_execution_ros.actions.ros_topic_monitor:RosTopicMonitor',
             'topic_publish = scenario_execution_ros.actions.ros_topic_publish:RosTopicPublish',
             'wait_for_data = scenario_execution_ros.actions.ros_topic_wait_for_data:RosTopicWaitForData',
+            'wait_for_nodes = scenario_execution_ros.actions.ros_wait_for_nodes:RosWaitForNodes',
             'wait_for_topics = scenario_execution_ros.actions.ros_topic_wait_for_topics:RosTopicWaitForTopics',
         ],
         'scenario_execution.osc_libraries': [

scenario_execution_ros/test/test_ros_wait_for_nodes.py

@@ -0,0 +1,94 @@
+# 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
+import threading
+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.utils.logging import Logger
+from antlr4.InputStream import InputStream
+
+os.environ["PYTHONUNBUFFERED"] = '1'
+
+
+class TestRosLogCheck(unittest.TestCase):
+    # pylint: disable=missing-function-docstring
+
+    def setUp(self):
+        rclpy.init()
+
+        self.received_msgs = []
+        self.node = rclpy.create_node('test_wait_for_nodes')
+
+        self.executor = rclpy.executors.MultiThreadedExecutor()
+        self.executor.add_node(self.node)
+        self.executor_thread = threading.Thread(target=self.executor.spin)
+        self.executor_thread.start()
+
+        self.callback_group = rclpy.callback_groups.ReentrantCallbackGroup()
+
+        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)
+        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):
+        self.node.destroy_node()
+        rclpy.try_shutdown()
+        self.executor_thread.join()
+
+    def test_success(self):
+        scenario_content = """
+import osc.ros
+
+scenario test_success:
+    do parallel:
+        serial:
+            wait_for_nodes(['/test_wait_for_nodes'])
+            emit end
+        time_out: serial:
+            wait elapsed(3s)
+            emit fail
+"""
+        self.execute(scenario_content)
+        self.assertTrue(self.scenario_execution_ros.process_results())
+
+    def test_timeout(self):
+        scenario_content = """
+import osc.ros
+
+scenario test_success:
+    do parallel:
+        serial:
+            wait_for_nodes(['UNKNOWN'])
+            emit end
+        time_out: serial:
+            wait elapsed(3s)
+            emit fail
+"""
+        self.execute(scenario_content)
+        self.assertFalse(self.scenario_execution_ros.process_results())