diff --git a/docs/libraries.rst b/docs/libraries.rst index 148b6ba4..bdcaf422 100644 --- a/docs/libraries.rst +++ b/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()`` ^^^^^^^^^^^^^^^^^^^^^ diff --git a/scenario_execution_ros/scenario_execution_ros/actions/ros_wait_for_nodes.py b/scenario_execution_ros/scenario_execution_ros/actions/ros_wait_for_nodes.py new file mode 100644 index 00000000..5cc838c8 --- /dev/null +++ b/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 diff --git a/scenario_execution_ros/scenario_execution_ros/lib_osc/ros.osc b/scenario_execution_ros/scenario_execution_ros/lib_osc/ros.osc index 679fa622..18ae923d 100644 --- a/scenario_execution_ros/scenario_execution_ros/lib_osc/ros.osc +++ b/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 diff --git a/scenario_execution_ros/setup.py b/scenario_execution_ros/setup.py index 92e0c2b5..5159efce 100644 --- a/scenario_execution_ros/setup.py +++ b/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': [ diff --git a/scenario_execution_ros/test/test_ros_wait_for_nodes.py b/scenario_execution_ros/test/test_ros_wait_for_nodes.py new file mode 100644 index 00000000..19bd46f5 --- /dev/null +++ b/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()) diff --git a/scenario_execution_rviz/README.md b/scenario_execution_rviz/README.md index 1430371f..9b5ba208 100644 --- a/scenario_execution_rviz/README.md +++ b/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 diff --git a/scenario_execution_rviz/src/scenario_view.cpp b/scenario_execution_rviz/src/scenario_view.cpp index 967fc830..835aebe3 100644 --- a/scenario_execution_rviz/src/scenario_view.cpp +++ b/scenario_execution_rviz/src/scenario_view.cpp @@ -16,12 +16,17 @@ #include "scenario_view.h" #include "indicator_widget.h" - +#include +#include 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( - "/scenario_execution/snapshots", 10, + mSnapshotTopic.toStdString().c_str(), 10, std::bind(&ScenarioView::behaviorTreeChanged, this, _1)); } diff --git a/scenario_execution_rviz/src/scenario_view.h b/scenario_execution_rviz/src/scenario_view.h index 9898a1fb..9d5ca1cf 100644 --- a/scenario_execution_rviz/src/scenario_view.h +++ b/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 *collapsedStates; + QString mSnapshotTopic; + QTimer mInitTimer; // icons QIcon runningIcon = QIcon(":/icons/chevron-right-o.png");