From 417a8f20ea84906eee70fa4fb1d6467d4d683b27 Mon Sep 17 00:00:00 2001 From: "Pasch, Frederik" Date: Sat, 2 Mar 2024 11:33:19 +0100 Subject: [PATCH] cleanup --- scenario_execution/README.md | 4 + scenario_execution/package.xml | 2 +- .../scenario_execution/actions/__init__.py | 2 - .../scenario_execution/actions/init_nav2.py | 5 +- .../scenario_execution/actions/nav2_common.py | 3 +- .../actions/nav_through_poses.py | 2 - .../scenario_execution/actions/nav_to_pose.py | 2 - .../actions/odometry_distance_traveled.py | 1 - .../actions/py_trees_ros_common.py | 3 - .../actions/ros_bag_record.py | 2 - .../actions/ros_log_check.py | 5 - .../actions/ros_service_call.py | 6 -- .../actions/ros_set_node_parameter.py | 8 -- .../actions/ros_topic_check_data.py | 17 +--- .../actions/ros_topic_publish.py | 11 +-- .../actions/ros_topic_wait_for_data.py | 4 +- .../actions/ros_topic_wait_for_topics.py | 5 - .../scenario_execution/actions/tf_close_to.py | 10 +- scenario_execution/scenarios/amr_demo.osc | 39 -------- .../test/test_ros_event_to_blackboard.osc | 13 --- .../test/test_ros_topic_from_blackboard.osc | 22 ----- .../test/test_ros_topic_to_blackboard.osc | 13 --- .../test/test_ros_event_to_blackboard.py | 68 ------------- .../test/test_ros_topic_from_blackboard.py | 74 -------------- .../test/test_ros_topic_to_blackboard.py | 68 ------------- .../scenario_execution_base/__init__.py | 2 - .../actions/__init__.py | 3 - .../actions/bool_expression.py | 43 -------- .../actions/open_port.py | 63 ------------ .../actions/run_external_process.py | 2 - .../actions/topic_equals.py | 52 ---------- .../actions/topic_publish.py | 58 ----------- .../actions/wait_for_open_ports.py | 97 ------------------- 33 files changed, 12 insertions(+), 697 deletions(-) delete mode 100644 scenario_execution/scenarios/amr_demo.osc delete mode 100644 scenario_execution/scenarios/test/test_ros_event_to_blackboard.osc delete mode 100644 scenario_execution/scenarios/test/test_ros_topic_from_blackboard.osc delete mode 100644 scenario_execution/scenarios/test/test_ros_topic_to_blackboard.osc delete mode 100644 scenario_execution/test/test_ros_event_to_blackboard.py delete mode 100644 scenario_execution/test/test_ros_topic_from_blackboard.py delete mode 100644 scenario_execution/test/test_ros_topic_to_blackboard.py delete mode 100644 scenario_execution_base/scenario_execution_base/actions/bool_expression.py delete mode 100644 scenario_execution_base/scenario_execution_base/actions/open_port.py delete mode 100644 scenario_execution_base/scenario_execution_base/actions/topic_equals.py delete mode 100644 scenario_execution_base/scenario_execution_base/actions/topic_publish.py delete mode 100644 scenario_execution_base/scenario_execution_base/actions/wait_for_open_ports.py diff --git a/scenario_execution/README.md b/scenario_execution/README.md index 82cb9202..a40f319a 100644 --- a/scenario_execution/README.md +++ b/scenario_execution/README.md @@ -1,3 +1,7 @@ # Scenario Execution The `scenario_execution` package is the ROS2 middleware implementation of the scenario execution. It uses the `py_trees_ros` packages as the `py_trees`'s implementation for ROS2. + +It provides the following scenario execution libraries: + +- `ros.osc`: ROS specific actions like topic publish and service call. diff --git a/scenario_execution/package.xml b/scenario_execution/package.xml index df600db1..2941d7c3 100644 --- a/scenario_execution/package.xml +++ b/scenario_execution/package.xml @@ -3,7 +3,7 @@ scenario_execution 1.0.0 - ROS Scenario Execution + Scenario Execution for ROS Intel Labs Intel Labs Apache-2.0 diff --git a/scenario_execution/scenario_execution/actions/__init__.py b/scenario_execution/scenario_execution/actions/__init__.py index 7fcb869f..3ba13780 100644 --- a/scenario_execution/scenario_execution/actions/__init__.py +++ b/scenario_execution/scenario_execution/actions/__init__.py @@ -13,5 +13,3 @@ # and limitations under the License. # # SPDX-License-Identifier: Apache-2.0 - -""" Entry for module action plugins """ diff --git a/scenario_execution/scenario_execution/actions/init_nav2.py b/scenario_execution/scenario_execution/actions/init_nav2.py index 72f3d54e..c20bace9 100644 --- a/scenario_execution/scenario_execution/actions/init_nav2.py +++ b/scenario_execution/scenario_execution/actions/init_nav2.py @@ -14,8 +14,6 @@ # # SPDX-License-Identifier: Apache-2.0 -""" Class for initializing nav2 by setting an initial pose and activate required nodes """ - from enum import Enum @@ -54,7 +52,8 @@ class InitNav2State(Enum): class InitNav2(py_trees.behaviour.Behaviour): """ - Class to initialize nav2 + Class for initializing nav2 by setting an initial pose and activate required nodes """ + """ def __init__(self, name, associated_actor, initial_pose: list, base_frame_id: str, wait_for_initial_pose: bool, use_initial_pose: bool, namespace_override: str): diff --git a/scenario_execution/scenario_execution/actions/nav2_common.py b/scenario_execution/scenario_execution/actions/nav2_common.py index be37a0d8..d10a50f8 100644 --- a/scenario_execution/scenario_execution/actions/nav2_common.py +++ b/scenario_execution/scenario_execution/actions/nav2_common.py @@ -1,3 +1,4 @@ +# Copyright 2021 Samsung Research America # Copyright (C) 2024 Intel Corporation # # Licensed under the Apache License, Version 2.0 (the "License"); @@ -14,8 +15,6 @@ # # SPDX-License-Identifier: Apache-2.0 -""" Subclass of BasicNavigator to support namespaces """ - from geometry_msgs.msg import PoseStamped, PoseWithCovarianceStamped from rclpy.qos import QoSDurabilityPolicy, QoSHistoryPolicy from rclpy.qos import QoSProfile, QoSReliabilityPolicy diff --git a/scenario_execution/scenario_execution/actions/nav_through_poses.py b/scenario_execution/scenario_execution/actions/nav_through_poses.py index e64110af..13f68b11 100644 --- a/scenario_execution/scenario_execution/actions/nav_through_poses.py +++ b/scenario_execution/scenario_execution/actions/nav_through_poses.py @@ -14,8 +14,6 @@ # # SPDX-License-Identifier: Apache-2.0 -""" Scenario execution plugin for navigating through poses """ - from enum import Enum from rclpy.node import Node diff --git a/scenario_execution/scenario_execution/actions/nav_to_pose.py b/scenario_execution/scenario_execution/actions/nav_to_pose.py index c56151f6..7651bb83 100644 --- a/scenario_execution/scenario_execution/actions/nav_to_pose.py +++ b/scenario_execution/scenario_execution/actions/nav_to_pose.py @@ -14,8 +14,6 @@ # # SPDX-License-Identifier: Apache-2.0 -""" Scenario execution plugin for navigating to pose """ - from enum import Enum from rclpy.node import Node diff --git a/scenario_execution/scenario_execution/actions/odometry_distance_traveled.py b/scenario_execution/scenario_execution/actions/odometry_distance_traveled.py index ec900150..54b61a18 100644 --- a/scenario_execution/scenario_execution/actions/odometry_distance_traveled.py +++ b/scenario_execution/scenario_execution/actions/odometry_distance_traveled.py @@ -14,7 +14,6 @@ # # SPDX-License-Identifier: Apache-2.0 -""" Scenario execution plugin for waiting for a certain covered distance, based on odometry """ from math import sqrt import rclpy from rclpy.logging import get_logger diff --git a/scenario_execution/scenario_execution/actions/py_trees_ros_common.py b/scenario_execution/scenario_execution/actions/py_trees_ros_common.py index 15147bbb..2eb402cf 100644 --- a/scenario_execution/scenario_execution/actions/py_trees_ros_common.py +++ b/scenario_execution/scenario_execution/actions/py_trees_ros_common.py @@ -14,8 +14,6 @@ # # SPDX-License-Identifier: Apache-2.0 -""" Class for recording a ros bag """ - import py_trees import py_trees_ros # pylint: disable=import-error import typing @@ -26,7 +24,6 @@ class SubscriberHandler(py_trees_ros.subscribers.Handler): """ overrides Handler """ - def __init__(self, name: str, topic_name: str, diff --git a/scenario_execution/scenario_execution/actions/ros_bag_record.py b/scenario_execution/scenario_execution/actions/ros_bag_record.py index 3cda1771..704c160e 100644 --- a/scenario_execution/scenario_execution/actions/ros_bag_record.py +++ b/scenario_execution/scenario_execution/actions/ros_bag_record.py @@ -14,8 +14,6 @@ # # SPDX-License-Identifier: Apache-2.0 -""" Class for recording a ros bag """ - import os from datetime import datetime from enum import Enum diff --git a/scenario_execution/scenario_execution/actions/ros_log_check.py b/scenario_execution/scenario_execution/actions/ros_log_check.py index f71a3795..03e2b587 100644 --- a/scenario_execution/scenario_execution/actions/ros_log_check.py +++ b/scenario_execution/scenario_execution/actions/ros_log_check.py @@ -14,8 +14,6 @@ # # SPDX-License-Identifier: Apache-2.0 -""" Scenario execution plugin to wait for a specific string in the ros log """ - import py_trees from rclpy.qos import QoSProfile, DurabilityPolicy, HistoryPolicy import rclpy @@ -26,9 +24,6 @@ class RosLogCheck(py_trees.behaviour.Behaviour): """ Class for scanning the ros log for specific content - - Args: - values [str]: Values to look for """ def __init__(self, name, values: list): diff --git a/scenario_execution/scenario_execution/actions/ros_service_call.py b/scenario_execution/scenario_execution/actions/ros_service_call.py index 542bdfd9..b0bb2a47 100644 --- a/scenario_execution/scenario_execution/actions/ros_service_call.py +++ b/scenario_execution/scenario_execution/actions/ros_service_call.py @@ -14,7 +14,6 @@ # # SPDX-License-Identifier: Apache-2.0 -""" Scenario execution plugin to put ros data into the blackboard """ from ast import literal_eval import importlib from enum import Enum @@ -37,11 +36,6 @@ class ServiceCallActionState(Enum): class RosServiceCall(py_trees.behaviour.Behaviour): """ ros service call behavior - - Args: - service_name: name of the topic to connect to - service_type: The service type - call: call content """ def __init__(self, name, service_name: str, service_type: str, data: str): diff --git a/scenario_execution/scenario_execution/actions/ros_set_node_parameter.py b/scenario_execution/scenario_execution/actions/ros_set_node_parameter.py index 9b7b9f1b..4d2e9c79 100644 --- a/scenario_execution/scenario_execution/actions/ros_set_node_parameter.py +++ b/scenario_execution/scenario_execution/actions/ros_set_node_parameter.py @@ -14,9 +14,6 @@ # # SPDX-License-Identifier: Apache-2.0 -""" -Behavior to set a ros node parameter -""" from ast import literal_eval from .ros_service_call import RosServiceCall @@ -26,11 +23,6 @@ class RosSetNodeParameter(RosServiceCall): """ class for setting a node parameter - - Args: - node_name: [str]: name of the node - parameter_name: [str]: name of the parameter - parameter_value: [str]: new value of the parameter """ def __init__(self, name, node_name: str, parameter_name: str, parameter_value: str): diff --git a/scenario_execution/scenario_execution/actions/ros_topic_check_data.py b/scenario_execution/scenario_execution/actions/ros_topic_check_data.py index fd7583bd..1ebc6de7 100644 --- a/scenario_execution/scenario_execution/actions/ros_topic_check_data.py +++ b/scenario_execution/scenario_execution/actions/ros_topic_check_data.py @@ -14,30 +14,15 @@ # # SPDX-License-Identifier: Apache-2.0 -""" Scenario execution plugin to check data on a ros topic """ - import importlib import py_trees_ros # pylint: disable=import-error -from scenario_execution.action_plugins.conversions import get_qos_preset_profile, \ +from scenario_execution.actions.conversions import get_qos_preset_profile, \ get_comparison_operator, get_clearing_policy class RosTopicCheckData(py_trees_ros.subscribers.CheckData): """ Class to check if the message on ROS topic equals to the target message - - Args: - topic_name: name of the topic to connect to - topic_type: class of the message type (e.g. std_msgs.msg.String) - qos_profile: qos profile for the subscriber - variable_name: name of the variable to check - expected_value: expected value of the variable - comparison_operator: one from the python `operator module`_ - fail_if_no_data: py_trees.common.Status.FAILURE instead - of py_trees.common.Status.RUNNING if there is no data yet - fail_if_bad_comparison: py_trees.common.Status.FAILURE instead of - py_trees.common.Status.RUNNING if comparison failed - clearing_policy: when to clear the data """ def __init__(self, diff --git a/scenario_execution/scenario_execution/actions/ros_topic_publish.py b/scenario_execution/scenario_execution/actions/ros_topic_publish.py index 6daf889c..ac8a6fb5 100644 --- a/scenario_execution/scenario_execution/actions/ros_topic_publish.py +++ b/scenario_execution/scenario_execution/actions/ros_topic_publish.py @@ -14,9 +14,6 @@ # # SPDX-License-Identifier: Apache-2.0 -""" -Behavior to publish a msg on ROS topic -""" import importlib from ast import literal_eval @@ -26,18 +23,12 @@ import py_trees from py_trees.common import Status -from scenario_execution.action_plugins.conversions import get_qos_preset_profile +from scenario_execution.actions.conversions import get_qos_preset_profile class RosTopicPublish(py_trees.behaviour.Behaviour): """ class for publish a message on a ROS topic - - Args: - topic_name [str]: name of the topic to connect to - topic_type [str]: class of the message type (e.g. std_msgs.msg.String) - qos_profile [str]: qos profile for the subscriber - value [str]: expected value of the variable """ def __init__(self, name, topic_type: str, topic_name: str, qos_profile: str, value: str diff --git a/scenario_execution/scenario_execution/actions/ros_topic_wait_for_data.py b/scenario_execution/scenario_execution/actions/ros_topic_wait_for_data.py index 99c5a7a4..1e0e84bc 100644 --- a/scenario_execution/scenario_execution/actions/ros_topic_wait_for_data.py +++ b/scenario_execution/scenario_execution/actions/ros_topic_wait_for_data.py @@ -14,11 +14,9 @@ # # SPDX-License-Identifier: Apache-2.0 -""" Scenario execution plugin to wait for data on a ros topic """ - import importlib import py_trees -from scenario_execution.action_plugins.conversions import get_qos_preset_profile, get_clearing_policy +from scenario_execution.actions.conversions import get_qos_preset_profile, get_clearing_policy import typing import rclpy.qos from .py_trees_ros_common import SubscriberHandler diff --git a/scenario_execution/scenario_execution/actions/ros_topic_wait_for_topics.py b/scenario_execution/scenario_execution/actions/ros_topic_wait_for_topics.py index d8d33312..88752b02 100644 --- a/scenario_execution/scenario_execution/actions/ros_topic_wait_for_topics.py +++ b/scenario_execution/scenario_execution/actions/ros_topic_wait_for_topics.py @@ -14,8 +14,6 @@ # # SPDX-License-Identifier: Apache-2.0 -""" Scenario execution plugin to wait for ros topics to get available """ - import py_trees from rclpy.node import Node @@ -23,9 +21,6 @@ class RosTopicWaitForTopics(py_trees.behaviour.Behaviour): """ Class to check if ROS topic are available - - Args: - topics[str]: name of the topics to get available """ def __init__(self, topics: list): diff --git a/scenario_execution/scenario_execution/actions/tf_close_to.py b/scenario_execution/scenario_execution/actions/tf_close_to.py index 71acb6a0..58201cf2 100644 --- a/scenario_execution/scenario_execution/actions/tf_close_to.py +++ b/scenario_execution/scenario_execution/actions/tf_close_to.py @@ -14,8 +14,6 @@ # # SPDX-License-Identifier: Apache-2.0 -""" Module for checking a robot is close to a reference point using localization""" - from math import sqrt import rclpy @@ -28,18 +26,12 @@ from tf2_ros.buffer import Buffer from tf2_ros import TransformException # pylint: disable= no-name-in-module -from scenario_execution.action_plugins.nav2_common import NamespacedTransformListener +from scenario_execution.actions.nav2_common import NamespacedTransformListener class TfCloseTo(py_trees.behaviour.Behaviour): """ class for distance condition in ROS Gazebo simulation - - Args: - namespace [str]: name of the robot - reference_point [str]: target point to measure distance from \ - in the form of 3 number array: [pos_x, pos_y, pos_z] - threshold [str]: threshold to the reference point """ def __init__( diff --git a/scenario_execution/scenarios/amr_demo.osc b/scenario_execution/scenarios/amr_demo.osc deleted file mode 100644 index 520f82b4..00000000 --- a/scenario_execution/scenarios/amr_demo.osc +++ /dev/null @@ -1,39 +0,0 @@ -############### -# Dependencies -############### - -import osc.ros -import osc.gazebo - -##################### -# Example scenario -##################### - -scenario spawn_obstacle: - robot_1: differential_drive_robot with: - keep(it.model == 'amr_simulation://robots/kobuki/pengo.urdf.xacro') - beer_bottle: amr_object with: - keep(it.model == 'amr_simulation://models/beer/model.sdf') - event robot1_arrives - do parallel: - robot1_arrives_at_ref_pt: serial: - robot_1_arrives: robot_1.close_to() with: - keep(it.reference_point.x == 1.0m) - keep(it.reference_point.y == 0.0m) - keep(it.reference_point.z == 0.0m) - keep(it.threshold == 0.3m) - emit_arrival: emit robot1_arrives - spawn_obstacle: serial: - wait_for_robot1_to_arrive: wait @spawn_obstacle.robot1_arrives - wait_for_2s: wait elapsed(2s) - spawn_beer_bottle: beer_bottle.spawn() with: - keep(it.spawn_pose.position.x == 1.0m) - keep(it.spawn_pose.position.y == -4.0m) - keep(it.spawn_pose.position.z == 0.0m) - keep(it.spawn_pose.orientation.roll == 0.0m) - keep(it.spawn_pose.orientation.pitch == 0.0m) - keep(it.spawn_pose.orientation.yaw == 0.0m) - scenario_finished: emit end - time_out: serial: - wait_for_30s: wait elapsed(30s) - time_out_shutdown: emit fail diff --git a/scenario_execution/scenarios/test/test_ros_event_to_blackboard.osc b/scenario_execution/scenarios/test/test_ros_event_to_blackboard.osc deleted file mode 100644 index 5a842161..00000000 --- a/scenario_execution/scenarios/test/test_ros_event_to_blackboard.osc +++ /dev/null @@ -1,13 +0,0 @@ -import osc.ros - -scenario test_ros_event_to_blackboard: - do parallel: - test: serial: - wait elapsed(2s) - event_to_blackboard() with: - keep(it.topic_name == '/bla') - keep(it.variable_name == '/test_ros_event_to_blackboard/end') - wait elapsed(60s) - time_out: serial: - wait_for_30s: wait elapsed(30s) - time_out_shutdown: emit fail diff --git a/scenario_execution/scenarios/test/test_ros_topic_from_blackboard.osc b/scenario_execution/scenarios/test/test_ros_topic_from_blackboard.osc deleted file mode 100644 index 1bfab462..00000000 --- a/scenario_execution/scenarios/test/test_ros_topic_from_blackboard.osc +++ /dev/null @@ -1,22 +0,0 @@ -import osc.ros - -scenario test_ros_topic_from_blackboard: - do parallel: - wait_for_msg: serial: - topic_to_blackboard() with: - keep(it.topic_name == '/bla') - keep(it.topic_type == 'std_msgs.msg.Bool') - keep(it.blackboard_variables == '/bla_blackboard') - publish_msg: serial: - wait_for_blackboard_variable() with: - keep(it.variable_name == '/bla_blackboard') - topic_from_blackboard() with: - keep(it.topic_name == '/final') - keep(it.topic_type == 'std_msgs.msg.Bool') - keep(it.blackboard_variable == '/bla_blackboard') - unset_blackboard_variable() with: - keep(it.key == '/bla_blackboard') - emit end - time_out: serial: - wait elapsed(30s) - time_out_shutdown: emit fail diff --git a/scenario_execution/scenarios/test/test_ros_topic_to_blackboard.osc b/scenario_execution/scenarios/test/test_ros_topic_to_blackboard.osc deleted file mode 100644 index f38b8a87..00000000 --- a/scenario_execution/scenarios/test/test_ros_topic_to_blackboard.osc +++ /dev/null @@ -1,13 +0,0 @@ -import osc.ros - -scenario test_ros_topic_to_blackboard: - do parallel: - test: serial: - topic_to_blackboard() with: - keep(it.topic_name == 'bla') - keep(it.topic_type == 'std_msgs.msg.Bool') - keep(it.blackboard_variables == '{ \"/test_ros_topic_to_blackboard/end\": \"data\" }') - emit end - time_out: serial: - wait_for_30s: wait elapsed(30s) - time_out_shutdown: emit fail diff --git a/scenario_execution/test/test_ros_event_to_blackboard.py b/scenario_execution/test/test_ros_event_to_blackboard.py deleted file mode 100644 index 05f3361e..00000000 --- a/scenario_execution/test/test_ros_event_to_blackboard.py +++ /dev/null @@ -1,68 +0,0 @@ -# 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 threading -from scenario_execution import ROSScenarioExecution -from scenario_execution_base.model.osc2_parser import OpenScenario2Parser -from scenario_execution_base.utils.logging import Logger -from ament_index_python.packages import get_package_share_directory - -from std_msgs.msg import Empty - -os.environ["PYTHONUNBUFFERED"] = '1' - - -class Test(unittest.TestCase): - # pylint: disable=missing-function-docstring,missing-class-docstring - @classmethod - def setUpClass(cls): - rclpy.init() - - @classmethod - def tearDownClass(cls): - rclpy.shutdown() - - def setUp(self) -> None: - self.parser = OpenScenario2Parser(Logger('test')) - self.scenario_execution = ROSScenarioExecution() - - self.scenario_dir = get_package_share_directory('scenario_execution') - - self.node = rclpy.create_node('test_node') - self.pub = self.node.create_publisher(Empty, "/bla", 10) - self.timer = self.node.create_timer(1, self.timer_callback) - - self.executor = rclpy.executors.MultiThreadedExecutor() - self.executor.add_node(self.node) - self.executor_thread = threading.Thread(target=self.executor.spin, daemon=True) - self.executor_thread.start() - - def tearDown(self): - self.node.destroy_node() - - def timer_callback(self): - self.pub.publish(Empty()) - - def test_success(self): - scenarios = self.parser.process_file(os.path.join( - self.scenario_dir, 'scenarios', 'test', 'test_ros_event_to_blackboard.osc'), False) - self.assertIsNotNone(scenarios) - self.scenario_execution.scenarios = scenarios - ret = self.scenario_execution.run() - self.assertTrue(ret) diff --git a/scenario_execution/test/test_ros_topic_from_blackboard.py b/scenario_execution/test/test_ros_topic_from_blackboard.py deleted file mode 100644 index f327792c..00000000 --- a/scenario_execution/test/test_ros_topic_from_blackboard.py +++ /dev/null @@ -1,74 +0,0 @@ -# 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 threading - -from ament_index_python.packages import get_package_share_directory - -import rclpy -from std_msgs.msg import Bool - -from scenario_execution import ROSScenarioExecution -from scenario_execution_base.model.osc2_parser import OpenScenario2Parser -from scenario_execution_base.utils.logging import Logger - - -class Test(unittest.TestCase): - # pylint: disable=missing-function-docstring,missing-class-docstring - @classmethod - def setUpClass(cls): - rclpy.init() - - @classmethod - def tearDownClass(cls): - rclpy.shutdown() - - def setUp(self) -> None: - self.received_msgs = [] - self.parser = OpenScenario2Parser(Logger('test')) - self.scenario_execution = ROSScenarioExecution() - - self.scenario_dir = get_package_share_directory('scenario_execution') - - self.node = rclpy.create_node('test_node') - self.pub = self.node.create_publisher(Bool, "/bla", 10) - self.sub = self.node.create_subscription(Bool, "/final", self.callback, 10) - self.timer = self.node.create_timer(3, self.timer_callback) - - self.executor = rclpy.executors.MultiThreadedExecutor() - self.executor.add_node(self.node) - self.executor_thread = threading.Thread(target=self.executor.spin, daemon=True) - self.executor_thread.start() - - def tearDown(self): - self.node.destroy_node() - - def timer_callback(self): - self.pub.publish(Bool(data=True)) - - def callback(self, msg): - self.received_msgs.append(msg) - - def test_success(self): - scenarios = self.parser.process_file(os.path.join( - self.scenario_dir, 'scenarios', 'test', 'test_ros_topic_from_blackboard.osc'), False) - self.assertIsNotNone(scenarios) - self.scenario_execution.scenarios = scenarios - ret = self.scenario_execution.run() - self.assertTrue(ret) - self.assertEqual(len(self.received_msgs), 1) diff --git a/scenario_execution/test/test_ros_topic_to_blackboard.py b/scenario_execution/test/test_ros_topic_to_blackboard.py deleted file mode 100644 index 2fc01811..00000000 --- a/scenario_execution/test/test_ros_topic_to_blackboard.py +++ /dev/null @@ -1,68 +0,0 @@ -# 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 threading - -from ament_index_python.packages import get_package_share_directory - -import rclpy -from std_msgs.msg import Bool - -from scenario_execution import ROSScenarioExecution -from scenario_execution_base.model.osc2_parser import OpenScenario2Parser -from scenario_execution_base.utils.logging import Logger - - -class Test(unittest.TestCase): - # pylint: disable=missing-function-docstring,missing-class-docstring - @classmethod - def setUpClass(cls): - rclpy.init() - - @classmethod - def tearDownClass(cls): - rclpy.shutdown() - - def setUp(self) -> None: - self.parser = OpenScenario2Parser(Logger('test')) - self.scenario_execution = ROSScenarioExecution() - - self.scenario_dir = get_package_share_directory('scenario_execution') - - self.node = rclpy.create_node('test_node') - self.pub = self.node.create_publisher(Bool, "/bla", 10) - self.timer = self.node.create_timer(3, self.timer_callback) - - self.executor = rclpy.executors.MultiThreadedExecutor() - self.executor.add_node(self.node) - self.executor_thread = threading.Thread(target=self.executor.spin, daemon=True) - self.executor_thread.start() - - def tearDown(self): - self.node.destroy_node() - - def timer_callback(self): - self.pub.publish(Bool(data=True)) - - def test_success(self): - scenarios = self.parser.process_file(os.path.join( - self.scenario_dir, 'scenarios', 'test', 'test_ros_topic_to_blackboard.osc'), False) - self.assertIsNotNone(scenarios) - self.scenario_execution.scenarios = scenarios - ret = self.scenario_execution.run() - self.assertTrue(ret) diff --git a/scenario_execution_base/scenario_execution_base/__init__.py b/scenario_execution_base/scenario_execution_base/__init__.py index b3fd9469..bbdeabb5 100644 --- a/scenario_execution_base/scenario_execution_base/__init__.py +++ b/scenario_execution_base/scenario_execution_base/__init__.py @@ -14,8 +14,6 @@ # # SPDX-License-Identifier: Apache-2.0 -"""Main entry point for the scenario execution base package """ - from . import actions from . import utils from . import model diff --git a/scenario_execution_base/scenario_execution_base/actions/__init__.py b/scenario_execution_base/scenario_execution_base/actions/__init__.py index 6731d6e5..4dd9f570 100644 --- a/scenario_execution_base/scenario_execution_base/actions/__init__.py +++ b/scenario_execution_base/scenario_execution_base/actions/__init__.py @@ -14,15 +14,12 @@ # # SPDX-License-Identifier: Apache-2.0 -"""behaviors module""" - from .bool_expression import BoolExpression from .topic_equals import TopicEquals from .topic_publish import TopicPublish from .run_external_process import RunExternalProcess __all__ = [ - 'BoolExpression', 'TopicEquals', 'TopicPublish', 'RunExternalProcess', diff --git a/scenario_execution_base/scenario_execution_base/actions/bool_expression.py b/scenario_execution_base/scenario_execution_base/actions/bool_expression.py deleted file mode 100644 index 178c66e8..00000000 --- a/scenario_execution_base/scenario_execution_base/actions/bool_expression.py +++ /dev/null @@ -1,43 +0,0 @@ -# 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 - -""" -Behavior for bool expression in event_condition -""" -import py_trees -from py_trees.common import Status - - -class BoolExpression(py_trees.behaviour.Behaviour): - """ - Class for bool expression in event_condition - - Args: - value [bool]: boolean value to check - """ - - def __init__(self, value: bool): - super().__init__(name=self.__class__.__name__) - self.value = value - - def update(self) -> Status: - """ - Return success if the bool value is true. - """ - if self.value: - return Status.SUCCESS - else: - return Status.FAILURE diff --git a/scenario_execution_base/scenario_execution_base/actions/open_port.py b/scenario_execution_base/scenario_execution_base/actions/open_port.py deleted file mode 100644 index cf79c522..00000000 --- a/scenario_execution_base/scenario_execution_base/actions/open_port.py +++ /dev/null @@ -1,63 +0,0 @@ -# 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 - -""" Scenario execution plugin to run an external command """ - -import py_trees # pylint: disable=import-error -from threading import Thread, Lock -import socket -import time - - -class OpenPort(py_trees.behaviour.Behaviour): - """ - Open a port (and keep it open until shutdown) - - Args: - command[str]: external command to execute - """ - - def __init__(self, name, port, address): - super().__init__(name) - self.port = port - self.address = address - self.thread = None - self.shutdown_requested = Lock() - - def update(self) -> py_trees.common.Status: - """ - return: - py_trees.common.Status - """ - if not self.thread: - def signal_ready(shutdown_requested): - with socket.socket(socket.AF_INET, socket.SOCK_STREAM) as s: - s.bind((self.address, self.port)) - s.listen() - while not shutdown_requested.locked(): - time.sleep(1) - self.thread = Thread(target=signal_ready, args=(self.shutdown_requested,)) - self.thread.start() - self.feedback_message = f"Port {self.port} is open." # pylint: disable= attribute-defined-outside-init - # The status might be reported to early (as listen() within thread might take time) - return py_trees.common.Status.SUCCESS - - def cleanup(self): - """ - Cleanup on shutdown - """ - self.shutdown_requested.acquire() - self.thread.join() diff --git a/scenario_execution_base/scenario_execution_base/actions/run_external_process.py b/scenario_execution_base/scenario_execution_base/actions/run_external_process.py index 498138a9..fcac0673 100644 --- a/scenario_execution_base/scenario_execution_base/actions/run_external_process.py +++ b/scenario_execution_base/scenario_execution_base/actions/run_external_process.py @@ -14,8 +14,6 @@ # # SPDX-License-Identifier: Apache-2.0 -""" Scenario execution plugin to run an external command """ - import py_trees # pylint: disable=import-error import subprocess # nosec B404 from threading import Thread diff --git a/scenario_execution_base/scenario_execution_base/actions/topic_equals.py b/scenario_execution_base/scenario_execution_base/actions/topic_equals.py deleted file mode 100644 index 2dc885b7..00000000 --- a/scenario_execution_base/scenario_execution_base/actions/topic_equals.py +++ /dev/null @@ -1,52 +0,0 @@ -# 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 - -""" -Behavior checking if the value matches the key in py_trees blackboard -""" -import py_trees - -from py_trees.common import Access, Status - - -class TopicEquals(py_trees.behaviour.Behaviour): - """ - Class to listen to a topic in Blackboard and check if it equals the defined message - - Args: - key [str]: topic to listen to - msg [str]: target message to match - namespace [str]: namespace of the key - """ - - def __init__(self, key: str, msg: str, namespace: str = None): - super().__init__(self.__class__.__name__) - - self.namespace = namespace - self.key = key - self.msg = msg - - self.client = self.attach_blackboard_client(namespace=self.namespace) - self.client.register_key(self.key, access=Access.READ) - - def update(self): - """ - Check the message on the topic equals the target message - """ - msg_on_blackboard = self.client.get(self.key) - if msg_on_blackboard == self.msg: - return Status.SUCCESS - return Status.RUNNING diff --git a/scenario_execution_base/scenario_execution_base/actions/topic_publish.py b/scenario_execution_base/scenario_execution_base/actions/topic_publish.py deleted file mode 100644 index b7ec36f1..00000000 --- a/scenario_execution_base/scenario_execution_base/actions/topic_publish.py +++ /dev/null @@ -1,58 +0,0 @@ -# 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 - -""" -Behavior to publish a message on py_trees blackboard -""" -import py_trees - -from py_trees.common import Access, Status - - -class TopicPublish(py_trees.behaviour.Behaviour): - """ - Class to publish a message to a topic - - Args: - key [str]: topic to publish on - msg [str]: message to publish on that topic - namespace [str]: namespace of the key - """ - - def __init__(self, name: "TopicPublish", key: str, msg: str, namespace: str = None): - super().__init__(name) - - self.namespace = namespace - self.key = key - self.msg = msg - - self.client = self.attach_blackboard_client(namespace=self.namespace) - self.client.register_key(self.key, access=Access.WRITE) - - def setup(self, **kwargs): - """ - Setup empty topic on blackboard - - This is to prevent the "Reader" from reading before the topic exists. - """ - self.client.set(self.key, '') - - def update(self): - """ - publish the message to topic - """ - self.client.set(self.key, self.msg) - return Status.SUCCESS diff --git a/scenario_execution_base/scenario_execution_base/actions/wait_for_open_ports.py b/scenario_execution_base/scenario_execution_base/actions/wait_for_open_ports.py deleted file mode 100644 index a64446ba..00000000 --- a/scenario_execution_base/scenario_execution_base/actions/wait_for_open_ports.py +++ /dev/null @@ -1,97 +0,0 @@ -# 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 - -""" Scenario execution plugin to wait for ports """ - -import py_trees # pylint: disable=import-error -from threading import Thread, Lock -import socket -import time -from collections import deque - - -class WaitForOpenPorts(py_trees.behaviour.Behaviour): - """ - Wait for network ports to open - - Args: - targets[str]: list of targets (host:port) - """ - - def __init__(self, name, targets): - super().__init__(name) - self.threads = [] - self.shutdown_requested = Lock() - self.targets = targets - self.targets_done = deque() - - def setup(self, **kwargs): - """ - parse - """ - tmp = self.targets.split(';') - self.targets = [] - for elem in tmp: - pair = elem.split(':') - if len(pair) != 2: - raise ValueError("Invalid pair") - self.targets.append((pair[0], pair[1])) - if len(self.targets) == 0: - raise ValueError( - f"Targets invalid. Expected format: :;:;.. Found {tmp}") - - def update(self) -> py_trees.common.Status: - """ - return: - py_trees.common.Status - """ - if not self.threads: - def wait_for_port(host, port, shutdown_requested): - while not shutdown_requested.locked(): - try: - with socket.create_connection((host, port)): - return True - except OSError: - time.sleep(1.) - return False - - for target in self.targets: - thread = Thread(target=wait_for_port, args=( - target[0], target[1], self.shutdown_requested), name=f"{target[0]};{target[1]}") - thread.start() - thread.done = False - self.threads.append(thread) - return py_trees.common.Status.RUNNING - - for t in self.threads: - if not t.is_alive(): - t.done = True - self.threads = [t for t in self.threads if not t.done] - running_desc = ";".join([t.name for t in self.threads]) - if self.threads: - self.feedback_message = f"Waiting for {running_desc}" # pylint: disable= attribute-defined-outside-init - return py_trees.common.Status.RUNNING - else: - self.feedback_message = "All ports found." # pylint: disable= attribute-defined-outside-init - return py_trees.common.Status.SUCCESS - - def cleanup(self): - """ - Cleanup on shutdown - """ - self.shutdown_requested.acquire() - for t in self.threads: - t.join()