Merge branch 'main' into fix_msg_conv

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

@@ -149,6 +149,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())

scenario_execution_rviz/README.md

@@ -9,9 +9,7 @@ The following image shows a snapshot of the rviz plugin during a run of the [exa
 
 ![tree_example.png](../docs/images/scenario_view_example.png)
 
-### Known Issues
-
-The Scenario View panel can not display the scenario's behavior tree if initialized while the robot navigation is already running.
+The plugin subscribes to `/scenario_execution/snapshots` by default. Double-click on the header to modify the topic.
 
 ### Icon Licence
 

scenario_execution_rviz/src/scenario_view.cpp

@@ -16,12 +16,17 @@
 
 #include "scenario_view.h"
 #include "indicator_widget.h"
-
+#include <QHeaderView>
+#include <QInputDialog>
 using std::placeholders::_1;
 
 namespace scenario_execution_rviz {
 
-ScenarioView::ScenarioView(QWidget *parent) : rviz_common::Panel(parent) {
+ScenarioView::ScenarioView(QWidget *parent)
+    : rviz_common::Panel(parent), mInitTimer(this) {
+  mInitTimer.setSingleShot(true);
+  connect(&mInitTimer, &QTimer::timeout, this, &ScenarioView::setupConnection);
+
   QVBoxLayout *layout = new QVBoxLayout;
 
   QFormLayout *formLayout = new QFormLayout;
@@ -51,14 +56,58 @@ ScenarioView::ScenarioView(QWidget *parent) : rviz_common::Panel(parent) {
           SLOT(handleItemCollapsed(QTreeWidgetItem *)));
   connect(mScenarioView, SIGNAL(itemExpanded(QTreeWidgetItem *)), this,
           SLOT(handleItemExpanded(QTreeWidgetItem *)));
+  connect(mScenarioView->header(), SIGNAL(sectionDoubleClicked(int)), this,
+          SLOT(onHeaderDoubleClicked(int)));
+}
+
+void ScenarioView::load(const rviz_common::Config &config) {
+  rviz_common::Panel::load(config);
+  QString topic;
+  if (config.mapGetString("snapshot_topic", &topic)) {
+
+    if (mSnapshotTopic != topic) {
+      mInitTimer.stop(); // stop init trigger
+      mSnapshotTopic = topic;
+      setupConnection();
+    }
+  }
+}
+
+void ScenarioView::save(rviz_common::Config config) const {
+  rviz_common::Panel::save(config);
+  config.mapSetValue("snapshot_topic", mSnapshotTopic);
+}
+
+void ScenarioView::onHeaderDoubleClicked(int idx) {
+  (void)idx;
+  bool ok;
+  QString text = QInputDialog::getText(this, "Configuration", "Snapshot Topic",
+                                       QLineEdit::Normal, mSnapshotTopic, &ok);
+  if (ok && !text.isEmpty() && (mSnapshotTopic != text)) {
+    mSnapshotTopic = text;
+    setupConnection();
+    Q_EMIT configChanged();
+  }
 }
 
 void ScenarioView::onInitialize() {
   _node = getDisplayContext()->getRosNodeAbstraction().lock()->get_raw_node();
+  mSnapshotTopic = "/scenario_execution/snapshots";
+  mInitTimer.start(250); // only executed if no config-load gets triggered.
+}
+
+void ScenarioView::setupConnection() {
+  if (mSnapshotTopic.isEmpty()) {
+    return;
+  }
+  if (mBehaviorTreeSubscriber) {
+    mBehaviorTreeSubscriber.reset();
+  }
 
+  qDebug() << "Subscribing to" << mSnapshotTopic;
   mBehaviorTreeSubscriber =
       _node->create_subscription<py_trees_ros_interfaces::msg::BehaviourTree>(
-          "/scenario_execution/snapshots", 10,
+          mSnapshotTopic.toStdString().c_str(), 10,
           std::bind(&ScenarioView::behaviorTreeChanged, this, _1));
 }
 

scenario_execution_rviz/src/scenario_view.h

@@ -64,9 +64,17 @@ class ScenarioView : public rviz_common::Panel {
 public:
   ScenarioView(QWidget *parent = 0);
 
+  virtual void load(const rviz_common::Config &config) override;
+  virtual void save(rviz_common::Config config) const override;
+
+Q_SIGNALS:
+  void doubleClickedA(int index);
+
 protected Q_SLOTS:
   void handleItemCollapsed(QTreeWidgetItem *collapsedItem);
   void handleItemExpanded(QTreeWidgetItem *expandedItem);
+  void onHeaderDoubleClicked(int idx);
+  void setupConnection();
 
 protected:
   virtual void onInitialize() override;
@@ -93,6 +101,8 @@ protected Q_SLOTS:
   TreeModel *mTreeModel;
   bool treeWidgetBuilt = false;
   QMap<QString, bool> *collapsedStates;
+  QString mSnapshotTopic;
+  QTimer mInitTimer;
 
   // icons
   QIcon runningIcon = QIcon(":/icons/chevron-right-o.png");