diff --git a/docs/libraries.rst b/docs/libraries.rst index a983567d..fe747c66 100644 --- a/docs/libraries.rst +++ b/docs/libraries.rst @@ -96,8 +96,12 @@ For debugging purposes, log a string using the available log mechanism. Run a process. Reports `running` while the process has not finished. -- ``command: string``: Command to execute +If ``wait_for_shutdown`` is ``false`` and the process is still running on scenario shutdown, ``shutdown_signal`` is sent. If the process does not shutdown within shutdown_timeout, ``signal.sigkill`` is sent. +- ``command: string``: Command to execute +- ``wait_for_shutdown: bool``: Wait for the process to be finished. If false, the action immediately finishes (default: ``true``) +- ``shutdown_signal: signal``: (Only used if ``wait_for_shutdown`` is ``false``) Signal that is sent if a process is still running on scenario shutdown (default: ``signal!sigterm``) +- ``shutdown_timeout: time``: (Only used if ``wait_for_shutdown`` is ``false``) time to wait between ``shutdown_signal`` and SIGKILL getting sent, if process is still running on scenario shutdown (default: ``10s``) OS -- @@ -112,7 +116,15 @@ Actions Report success if a file exists. -- ``file_name: string``: File name to check +- ``file_name: string``: File to check + + +``check_file_not_exists()`` +""""""""""""""""""""""""""" + +Report success if a file does not exist. + +- ``file_name: string``: File to check Robotics @@ -270,6 +282,17 @@ Record a ROS bag, stored in directory ``output_dir`` defined by command-line par - ``hidden_topics: bool``: Whether to record hidden topics (default: ``false``) - ``storage: string``: Storage type to use (empty string: use ROS bag record default) +``ros_launch()`` +"""""""""""""""" + +Execute a ROS launch file. + +- ``package_name: string``: Package that contains the launch file +- ``launch_file: string``: Launch file name +- ``arguments: list of ros_argument``: ROS arguments (get forwarded as key:=value pairs) +- ``wait_for_shutdown: bool``: If true, the action waits until the execution is finished (default: ``true``) +- ``shutdown_timeout: time``: (Only used ``if wait_for_shutdown`` is ``false``) Time to wait between ``SIGINT`` and ``SIGKILL`` getting sent, if process is still running on scenario shutdown (default: ``10s``) + ``service_call()`` """""""""""""""""" diff --git a/libs/scenario_execution_os/scenario_execution_os/actions/check_file_exists.py b/libs/scenario_execution_os/scenario_execution_os/actions/check_file_exists.py index 0e18137f..a5bb3d09 100644 --- a/libs/scenario_execution_os/scenario_execution_os/actions/check_file_exists.py +++ b/libs/scenario_execution_os/scenario_execution_os/actions/check_file_exists.py @@ -29,6 +29,8 @@ def __init__(self, name, file_name): def update(self) -> py_trees.common.Status: if os.path.isfile(self.file_name): + self.feedback_message = f"File '{self.file_name}' exists" # pylint: disable= attribute-defined-outside-init return py_trees.common.Status.SUCCESS else: + self.feedback_message = f"File '{self.file_name}' does not exist" # pylint: disable= attribute-defined-outside-init return py_trees.common.Status.FAILURE diff --git a/libs/scenario_execution_os/scenario_execution_os/actions/check_file_not_exists.py b/libs/scenario_execution_os/scenario_execution_os/actions/check_file_not_exists.py new file mode 100644 index 00000000..272f1953 --- /dev/null +++ b/libs/scenario_execution_os/scenario_execution_os/actions/check_file_not_exists.py @@ -0,0 +1,36 @@ +# 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 py_trees + + +class CheckFileNotExists(py_trees.behaviour.Behaviour): + """ + Check that a file does not exist + """ + + def __init__(self, name, file_name): + super().__init__(name) + self.file_name = file_name + + def update(self) -> py_trees.common.Status: + if os.path.isfile(self.file_name): + self.feedback_message = f"File '{self.file_name}' exists" # pylint: disable= attribute-defined-outside-init + return py_trees.common.Status.FAILURE + else: + self.feedback_message = f"File '{self.file_name}' does not exist" # pylint: disable= attribute-defined-outside-init + return py_trees.common.Status.SUCCESS diff --git a/libs/scenario_execution_os/scenario_execution_os/lib_osc/os.osc b/libs/scenario_execution_os/scenario_execution_os/lib_osc/os.osc index 621da517..2a9feb40 100644 --- a/libs/scenario_execution_os/scenario_execution_os/lib_osc/os.osc +++ b/libs/scenario_execution_os/scenario_execution_os/lib_osc/os.osc @@ -3,3 +3,7 @@ import osc.standard.base action check_file_exists: # report success if a file exists file_name: string # file to check + +action check_file_not_exists: + # report success if a file does not exist + file_name: string # file to check diff --git a/libs/scenario_execution_os/setup.py b/libs/scenario_execution_os/setup.py index f22aabea..befb258e 100644 --- a/libs/scenario_execution_os/setup.py +++ b/libs/scenario_execution_os/setup.py @@ -39,6 +39,7 @@ entry_points={ 'scenario_execution.actions': [ 'check_file_exists = scenario_execution_os.actions.check_file_exists:CheckFileExists', + 'check_file_not_exists = scenario_execution_os.actions.check_file_not_exists:CheckFileNotExists', ], 'scenario_execution.osc_libraries': [ 'os = ' diff --git a/libs/scenario_execution_os/test/test_check_file_not_exists.py b/libs/scenario_execution_os/test/test_check_file_not_exists.py new file mode 100644 index 00000000..d784165d --- /dev/null +++ b/libs/scenario_execution_os/test/test_check_file_not_exists.py @@ -0,0 +1,77 @@ +# 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 unittest +import tempfile +from scenario_execution import ScenarioExecution +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 + + +class TestCheckData(unittest.TestCase): + # pylint: disable=missing-function-docstring,missing-class-docstring + + def setUp(self) -> None: + self.parser = OpenScenario2Parser(Logger('test', False)) + self.scenario_execution = ScenarioExecution(debug=False, log_model=False, live_tree=False, + scenario_file="test.osc", output_dir=None) + self.tmp_file = tempfile.NamedTemporaryFile() + print(self.tmp_file.name) + + def test_success(self): + scenario_content = """ +import osc.os + +scenario test: + do parallel: + serial: + check_file_not_exists('UNKNOWN_FILE') + emit end + time_out: serial: + wait elapsed(1s) + emit fail +""" + + parsed_tree = self.parser.parse_input_stream(InputStream(scenario_content)) + model = self.parser.create_internal_model(parsed_tree, "test.osc", False) + scenarios = create_py_tree(model, self.parser.logger, False) + self.scenario_execution.scenarios = scenarios + self.scenario_execution.run() + self.assertTrue(self.scenario_execution.process_results()) + + def test_fail(self): + scenario_content = """ +import osc.os + +scenario test: + do parallel: + serial: + check_file_not_exists('""" + self.tmp_file.name + """') + emit end + time_out: serial: + wait elapsed(1s) + emit fail +""" + + parsed_tree = self.parser.parse_input_stream(InputStream(scenario_content)) + model = self.parser.create_internal_model(parsed_tree, "test.osc", False) + scenarios = create_py_tree(model, self.parser.logger, False) + self.scenario_execution.scenarios = scenarios + self.scenario_execution.run() + self.assertFalse(self.scenario_execution.process_results()) diff --git a/scenario_execution/scenario_execution/actions/run_process.py b/scenario_execution/scenario_execution/actions/run_process.py index 38cb24ea..0422e0ba 100644 --- a/scenario_execution/scenario_execution/actions/run_process.py +++ b/scenario_execution/scenario_execution/actions/run_process.py @@ -18,19 +18,20 @@ import subprocess # nosec B404 from threading import Thread from collections import deque +import signal class RunProcess(py_trees.behaviour.Behaviour): """ - Class to execute an process. It finishes when the process finishes - - Args: - command[str]: command to execute + Class to execute an process. """ - def __init__(self, name, command=None): + def __init__(self, name, command=None, wait_for_shutdown=True, shutdown_timeout=10, shutdown_signal=("", signal.SIGTERM)): super().__init__(name) self.command = command.split(" ") if isinstance(command, str) else command + self.wait_for_shutdown = wait_for_shutdown + self.shutdown_timeout = shutdown_timeout + self.shutdown_signal = shutdown_signal[1] self.executed = False self.process = None self.log_stdout_thread = None @@ -78,6 +79,7 @@ def log_output(out, log_fct, buffer): self.log_stderr_thread.start() if self.process is None: + self.process = None return py_trees.common.Status.FAILURE ret = self.process.poll() @@ -106,7 +108,10 @@ def check_running_process(self): return: py_trees.common.Status """ - return py_trees.common.Status.RUNNING + if self.wait_for_shutdown: + return py_trees.common.Status.RUNNING + else: + return py_trees.common.Status.SUCCESS def on_process_finished(self, ret): """ @@ -133,3 +138,19 @@ def set_command(self, command): def get_command(self): return self.command + + def shutdown(self): + if self.process is None: + return + + ret = self.process.poll() + if ret is None: + # kill running process + self.logger.info(f'Sending {signal.Signals(self.shutdown_signal).name} to process...') + self.process.send_signal(self.shutdown_signal) + self.process.wait(self.shutdown_timeout) + if self.process.poll() is None: + self.logger.info('Sending SIGKILL to process...') + self.process.send_signal(signal.SIGKILL) + self.process.wait() + self.logger.info('Process finished.') diff --git a/scenario_execution/scenario_execution/lib_osc/helpers.osc b/scenario_execution/scenario_execution/lib_osc/helpers.osc index 0a6ebaad..a24769cb 100644 --- a/scenario_execution/scenario_execution/lib_osc/helpers.osc +++ b/scenario_execution/scenario_execution/lib_osc/helpers.osc @@ -1,11 +1,24 @@ +import osc.standard.base + +enum signal: [ + sighup = 1, + sigint = 2, + sigkill = 9, + sigusr1 = 10, + sigusr2 = 12, + sigterm = 15 +] action log: # Print out a message msg: string # Message to print action run_process: - # Run an external process + # Run an external process. If wait_for_shutdown is false and the process is still running on scenario shutdown, shutdown_signal is sent. If the process does not shutdown within shutdown_timeout, SIGKILL is sent. command: string # Command to execute + wait_for_shutdown: bool = true # wait for the process to be finished. If false, the action immediately finishes. + shutdown_signal: signal = signal!sigterm # (only used if wait_for_shutdown is false) signal that is sent if a process is still running on scenario shutdown + shutdown_timeout: time = 10s # (only used if wait_for_shutdown is false) time to wait between shutdown_signal and SIGKILL getting sent if process is still running on scenario shutdown struct random: def seed(seed_value: int = 0) is external scenario_execution.external_methods.random.seed() diff --git a/scenario_execution/test/test_run_process.py b/scenario_execution/test/test_run_process.py index 471e5b39..1b3f2474 100644 --- a/scenario_execution/test/test_run_process.py +++ b/scenario_execution/test/test_run_process.py @@ -21,6 +21,7 @@ from scenario_execution.model.model_to_py_tree import create_py_tree from scenario_execution.utils.logging import Logger from antlr4.InputStream import InputStream +from datetime import datetime class TestScenarioExecutionSuccess(unittest.TestCase): @@ -102,3 +103,41 @@ def test_multi_element_command(self): self.scenario_execution.scenarios = scenarios self.scenario_execution.run() self.assertTrue(self.scenario_execution.process_results()) + + def test_wait_for_shutdown_false(self): + scenario_content = """ +import osc.standard.base +import osc.helpers + +scenario test_run_process: + do parallel: + serial: + run_process('sleep 15', wait_for_shutdown: false) + emit end + time_out: serial: + wait elapsed(10s) + time_out_shutdown: emit fail +""" + parsed_tree = self.parser.parse_input_stream(InputStream(scenario_content)) + model = self.parser.create_internal_model(parsed_tree, "test.osc", False) + scenarios = create_py_tree(model, self.parser.logger, False) + self.scenario_execution.scenarios = scenarios + + start = datetime.now() + self.scenario_execution.run() + end = datetime.now() + duration = (end-start).total_seconds() + self.assertLessEqual(duration, 10.) + self.assertTrue(self.scenario_execution.process_results()) + + def test_signal_parsing(self): + scenario_content = """ +import osc.standard.base +import osc.helpers + +scenario test_run_process: + do run_process('sleep 15', wait_for_shutdown: false, shutdown_signal: signal!sigint) +""" + parsed_tree = self.parser.parse_input_stream(InputStream(scenario_content)) + model = self.parser.create_internal_model(parsed_tree, "test.osc", False) + _ = create_py_tree(model, self.parser.logger, False) diff --git a/scenario_execution_ros/scenario_execution_ros/actions/ros_launch.py b/scenario_execution_ros/scenario_execution_ros/actions/ros_launch.py new file mode 100644 index 00000000..9f7885f6 --- /dev/null +++ b/scenario_execution_ros/scenario_execution_ros/actions/ros_launch.py @@ -0,0 +1,53 @@ +# 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 + +from enum import Enum + +from scenario_execution.actions.run_process import RunProcess +import signal + + +class RosLaunchActionState(Enum): + """ + States for executing a ros bag recording + """ + WAITING_FOR_TOPICS = 1 + RECORDING = 2 + FAILURE = 5 + + +class RosLaunch(RunProcess): + """ + Class to execute ros bag recording + """ + + def __init__(self, name, package_name: str, launch_file: str, arguments: list, wait_for_shutdown: bool, shutdown_timeout: float): + super().__init__(name, None, wait_for_shutdown, shutdown_timeout, shutdown_signal=("", signal.SIGINT)) + self.package_name = package_name + self.launch_file = launch_file + self.arguments = arguments + self.wait_for_shutdown = wait_for_shutdown + self.command = None + + def setup(self, **kwargs): + self.command = ["ros2", "launch", self.package_name, self.launch_file] + + for arg in self.arguments: + if not arg["key"] or not arg["value"]: + raise ValueError(f'Invalid ros argument key:{arg["key"]}, value:{arg["value"]}') + self.command.append(f'{arg["key"]}:={arg["value"]}') + + self.logger.info(f'Command: {" ".join(self.command)}') 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 4add0c73..cf75c40c 100644 --- a/scenario_execution_ros/scenario_execution_ros/lib_osc/ros.osc +++ b/scenario_execution_ros/scenario_execution_ros/lib_osc/ros.osc @@ -1,5 +1,9 @@ import osc.robotics +struct ros_argument: + key: string + value: string = '' + # Policy rules for behaviours to dictate when data should be cleared/reset. # on_initialise Clear when entering the py_trees.behaviour.Behaviour.initialise method. # on_success Clear when returning py_trees.common.Status.SUCCESS @@ -122,6 +126,14 @@ action record_bag: hidden_topics: bool = false # whether to record hidden topics storage: string = '' # storage type to use (empty string: use default) +action ros_launch: + # Execute a ros launch file + package_name: string # package that contains the launch file + launch_file: string # launch file name + arguments: list of ros_argument # ros arguments (get forwarded as key:=value pairs) + wait_for_shutdown: bool = true # if true, the action waits until the execution has finished + shutdown_timeout: time = 10s # (only used if wait_for_shutdown is false) time to wait between SIGINT and SIGKILL getting sent, if process is still running on scenario shutdown + action service_call: service_name: string # name of the service to connect to service_type: string # class of the message type (e.g. std_srvs.msg.Empty) diff --git a/scenario_execution_ros/setup.py b/scenario_execution_ros/setup.py index c72ea982..efad3f54 100644 --- a/scenario_execution_ros/setup.py +++ b/scenario_execution_ros/setup.py @@ -57,6 +57,7 @@ 'differential_drive_robot.odometry_distance_traveled = scenario_execution_ros.actions.odometry_distance_traveled:OdometryDistanceTraveled', 'set_node_parameter = scenario_execution_ros.actions.ros_set_node_parameter:RosSetNodeParameter', 'record_bag = scenario_execution_ros.actions.ros_bag_record:RosBagRecord', + 'ros_launch = scenario_execution_ros.actions.ros_launch:RosLaunch', 'wait_for_topics = scenario_execution_ros.actions.ros_topic_wait_for_topics:RosTopicWaitForTopics', 'log_check = scenario_execution_ros.actions.ros_log_check:RosLogCheck', 'differential_drive_robot.tf_close_to = scenario_execution_ros.actions.tf_close_to:TfCloseTo', diff --git a/test/test_scenario_execution_ros/README.md b/test/test_scenario_execution_ros/README.md new file mode 100644 index 00000000..375a77e4 --- /dev/null +++ b/test/test_scenario_execution_ros/README.md @@ -0,0 +1,3 @@ +# Tests of Scenario Execution Library for ROS + +The `test_scenario_execution_ros` package tests functionality of `scenario_execution_ros`. diff --git a/test/test_scenario_execution_ros/launch/test_launch.py b/test/test_scenario_execution_ros/launch/test_launch.py new file mode 100644 index 00000000..4061631e --- /dev/null +++ b/test/test_scenario_execution_ros/launch/test_launch.py @@ -0,0 +1,46 @@ +# 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 + +from launch_ros.actions import Node +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration + + +def generate_launch_description(): + + test_param = LaunchConfiguration('test_param') + test_path = LaunchConfiguration('test_path') + timeout = LaunchConfiguration('timeout') + + return LaunchDescription([ + DeclareLaunchArgument('test_param', description='test parameter'), + DeclareLaunchArgument('test_path', description='Test path parameter'), + DeclareLaunchArgument('timeout', description='Timeout', default_value='5'), + + Node( + # condition=IfCondition(scenario_status), + package='test_scenario_execution_ros', + executable='test_scenario_execution_ros', + name='test_scenario_execution_ros', + parameters=[{ + 'test_param': test_param, + 'test_path': test_path, + 'timeout': timeout, + }], + output='screen' + ), + ]) diff --git a/test/test_scenario_execution_ros/package.xml b/test/test_scenario_execution_ros/package.xml new file mode 100644 index 00000000..89068f8b --- /dev/null +++ b/test/test_scenario_execution_ros/package.xml @@ -0,0 +1,22 @@ + + + + test_scenario_execution_ros + 1.1.0 + Tests for Scenario Execution library for ROS + Intel Labs + Intel Labs + Apache-2.0 + + scenario_execution_ros + scenario_execution_os + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/test/test_scenario_execution_ros/resource/test_scenario_execution_ros b/test/test_scenario_execution_ros/resource/test_scenario_execution_ros new file mode 100644 index 00000000..e69de29b diff --git a/test/test_scenario_execution_ros/setup.cfg b/test/test_scenario_execution_ros/setup.cfg new file mode 100644 index 00000000..fce95ccc --- /dev/null +++ b/test/test_scenario_execution_ros/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/test_scenario_execution_ros +[install] +install_scripts=$base/lib/test_scenario_execution_ros diff --git a/test/test_scenario_execution_ros/setup.py b/test/test_scenario_execution_ros/setup.py new file mode 100644 index 00000000..49965f87 --- /dev/null +++ b/test/test_scenario_execution_ros/setup.py @@ -0,0 +1,45 @@ +# 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 + +""" Setup python package """ +from glob import glob +import os +from setuptools import find_namespace_packages, setup + +PACKAGE_NAME = 'test_scenario_execution_ros' + +setup( + name=PACKAGE_NAME, + version='1.1.0', + packages=find_namespace_packages(), + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + PACKAGE_NAME]), + ('share/' + PACKAGE_NAME, ['package.xml']), + (os.path.join('share', PACKAGE_NAME, 'launch'), glob('launch/*launch.py')), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='Intel Labs', + maintainer_email='scenario-execution@intel.com', + description='Tests for Scenario Execution library for ROS', + license='Apache License 2.0', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + 'test_scenario_execution_ros = test_scenario_execution_ros.workload_node:main', + ]} +) diff --git a/test/test_scenario_execution_ros/test/test_ros_launch.py b/test/test_scenario_execution_ros/test/test_ros_launch.py new file mode 100644 index 00000000..179536f6 --- /dev/null +++ b/test/test_scenario_execution_ros/test/test_ros_launch.py @@ -0,0 +1,140 @@ +# 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 tempfile +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 TestScenarioExectionSuccess(unittest.TestCase): + # pylint: disable=missing-function-docstring + + def setUp(self): + rclpy.init() + self.request_received = None + self.node = rclpy.create_node('test_node_action') + + 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.parser = OpenScenario2Parser(Logger('test', False)) + self.scenario_execution_ros = ROSScenarioExecution() + self.tmp_dir = tempfile.TemporaryDirectory() + + def tearDown(self): + self.node.destroy_node() + rclpy.try_shutdown() + self.executor_thread.join() + self.tmp_dir.cleanup() + + def test_success(self): + scenario_content = """ +import osc.ros +import osc.os + +scenario test: + do parallel: + serial: + ros_launch('test_scenario_execution_ros', 'test_launch.py', [ + ros_argument(key: 'test_param', value: '""" + self.tmp_dir.name + """'), + ros_argument(key: 'test_path', value: '""" + self.tmp_dir.name + """') + ]) + check_file_exists(file_name: '""" + self.tmp_dir.name + '/test_started' + """') + check_file_exists(file_name: '""" + self.tmp_dir.name + '/test_success' + """') + check_file_not_exists(file_name: '""" + self.tmp_dir.name + '/test_aborted' + """') + emit end + time_out: serial: + wait elapsed(10s) + emit fail +""" + parsed_tree = self.parser.parse_input_stream(InputStream(scenario_content)) + model = self.parser.create_internal_model(parsed_tree, "test.osc", False) + scenarios = create_py_tree(model, self.parser.logger, False) + self.scenario_execution_ros.scenarios = scenarios + self.scenario_execution_ros.run() + self.assertTrue(self.scenario_execution_ros.process_results()) + + def test_success_not_wait_for_shutdown(self): + scenario_content = """ +import osc.ros +import osc.os + +scenario test: + do parallel: + serial: + ros_launch('test_scenario_execution_ros', 'test_launch.py', [ + ros_argument(key: 'test_param', value: '""" + self.tmp_dir.name + """'), + ros_argument(key: 'test_path', value: '""" + self.tmp_dir.name + """') + ], + wait_for_shutdown: false) + wait elapsed(4s) + check_file_exists(file_name: '""" + self.tmp_dir.name + '/test_started' + """') + check_file_not_exists(file_name: '""" + self.tmp_dir.name + '/test_success' + """') + check_file_not_exists(file_name: '""" + self.tmp_dir.name + '/test_aborted' + """') + wait elapsed(10s) + check_file_exists(file_name: '""" + self.tmp_dir.name + '/test_started' + """') + check_file_exists(file_name: '""" + self.tmp_dir.name + '/test_success' + """') + check_file_not_exists(file_name: '""" + self.tmp_dir.name + '/test_aborted' + """') + emit end + time_out: serial: + wait elapsed(20s) + emit fail +""" + parsed_tree = self.parser.parse_input_stream(InputStream(scenario_content)) + model = self.parser.create_internal_model(parsed_tree, "test.osc", False) + scenarios = create_py_tree(model, self.parser.logger, False) + self.scenario_execution_ros.scenarios = scenarios + self.scenario_execution_ros.run() + self.assertTrue(self.scenario_execution_ros.process_results()) + + def test_success_not_wait_for_shutdown_terminate(self): + scenario_content = """ +import osc.ros +import osc.os + +scenario test: + do serial: + ros_launch('test_scenario_execution_ros', 'test_launch.py', [ + ros_argument(key: 'test_param', value: '""" + self.tmp_dir.name + """'), + ros_argument(key: 'test_path', value: '""" + self.tmp_dir.name + """'), + ros_argument(key: 'timeout', value: '15') + ], + wait_for_shutdown: false, + shutdown_timeout: 5s) + wait elapsed(2s) + emit end +""" + parsed_tree = self.parser.parse_input_stream(InputStream(scenario_content)) + model = self.parser.create_internal_model(parsed_tree, "test.osc", False) + scenarios = create_py_tree(model, self.parser.logger, False) + self.scenario_execution_ros.scenarios = scenarios + self.scenario_execution_ros.run() + self.assertTrue(os.path.isfile(self.tmp_dir.name + '/test_started')) + self.assertFalse(os.path.isfile(self.tmp_dir.name + '/test_success')) + self.assertTrue(os.path.isfile(self.tmp_dir.name + '/test_aborted')) + self.assertTrue(self.scenario_execution_ros.process_results()) diff --git a/test/test_scenario_execution_ros/test_scenario_execution_ros/workload_node.py b/test/test_scenario_execution_ros/test_scenario_execution_ros/workload_node.py new file mode 100644 index 00000000..f70d49ab --- /dev/null +++ b/test/test_scenario_execution_ros/test_scenario_execution_ros/workload_node.py @@ -0,0 +1,70 @@ +# 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 rclpy +from rclpy.node import Node + +import sys + + +class TestNode(Node): + + def __init__(self): + super().__init__('test_node') + self.declare_parameter('test_path', rclpy.Parameter.Type.STRING) + self.declare_parameter('test_param', rclpy.Parameter.Type.STRING) + self.declare_parameter('timeout', rclpy.Parameter.Type.INTEGER) + self.test_path = self.get_parameter_or('test_path', '.').value + self.test_param = self.get_parameter_or('test_param', '').value + self.timeout = self.get_parameter_or('timeout', 5).value + self.get_logger().info(f"Writing to '{self.test_path}'... (timeout: {self.timeout})") + if not os.path.exists(self.test_path): + os.makedirs(self.test_path) + with open(os.path.join(self.test_path, 'test_started'), 'w') as f: + f.write(self.test_param) + self.timer = self.create_timer(self.timeout, self.timer_callback) + + def timer_callback(self): + self.get_logger().info('Timeout') + raise SystemExit() + + +def main(args=None): + rclpy.init(args=args) + + result = True + try: + node = TestNode() + + rclpy.spin(node) + except SystemExit: + pass + except BaseException: # pylint: disable=broad-exception-caught + result = False + + if result: + print("Success!") + with open(os.path.join(node.test_path, 'test_success'), 'w') as f: + f.write(node.test_param) + else: + print("Aborted!", file=sys.stderr) + with open(os.path.join(node.test_path, 'test_aborted'), 'w') as f: + f.write(node.test_param) + + +if __name__ == '__main__': + main()