diff --git a/rosbot_gazebo/test/test_multirobot_diff_drive_simulation.py b/rosbot_gazebo/test/test_multirobot_diff_drive_simulation.py new file mode 100644 index 00000000..572c5b47 --- /dev/null +++ b/rosbot_gazebo/test/test_multirobot_diff_drive_simulation.py @@ -0,0 +1,126 @@ +# Copyright 2021 Open Source Robotics Foundation, Inc. +# Copyright 2023 Intel Corporation. All Rights Reserved. +# Copyright 2023 Husarion +# +# 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. + +import launch_pytest +import pytest +import rclpy +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import ExecuteProcess +from rclpy.executors import MultiThreadedExecutor +from threading import Thread +from launch_testing.actions import ReadyToTest +from launch_testing.util import KeepAliveProc + +from test_utils import SimulationTestNode +from test_ign_kill_utils import kill_ign_linux_processes + + +@launch_pytest.fixture +def generate_test_description(): + # This is necessary to get unbuffered output from the process under test + proc_env = os.environ.copy() + proc_env["PYTHONUNBUFFERED"] = "1" + + # IncludeLaunchDescription does not work with robots argument + simulation_launch = ExecuteProcess( + cmd=[ + "ros2", + "launch", + "rosbot_gazebo", + "multirobot_simulation.launch.py", + ( + f'world:={get_package_share_directory("husarion_office_gz")}' + "/worlds/empty_with_plugins.sdf" + ), + "robots:=robot1={y: 0.0}; robot2={y: 1.0}; robot3={y: 2.0}; robot4={y: 3.0}", + "headless:=True", + ], + output="screen", + ) + + return LaunchDescription( + [ + simulation_launch, + KeepAliveProc(), + # Tell launch to start the test + ReadyToTest(), + ] + ) + + +@pytest.mark.launch(fixture=generate_test_description) +def test_multirobot_simulation(): + robot_names = ["robot1", "robot2", "robot3", "robot4"] + rclpy.init() + try: + nodes = [] + executor = MultiThreadedExecutor(num_threads=len(robot_names)) + + for robot_name in robot_names: + node = SimulationTestNode("test_simulation", namespace=robot_name) + node.create_test_subscribers_and_publishers() + nodes.append(node) + executor.add_node(node) + + ros_spin_thread = Thread(target=lambda executor: executor.spin(), args=(executor,)) + ros_spin_thread.start() + + for node in nodes: + # 0.9 m/s and 3.0 rad/s are controller's limits defined in + # rosbot_controller/config/mecanum_drive_controller.yaml + node.set_destination_speed(0.9, 0.0, 0.0) + + for node in nodes: + assert node.vel_stabilization_time_event.wait(timeout=60.0), ( + "The simulation is running slowly or has crashed! The time elapsed since setting" + f" the target speed is: {(node.current_time - node.goal_received_time):.1f}." + ) + assert ( + node.controller_odom_flag + ), "ROSbot does not move properly in x direction. Check rosbot_base_controller!" + assert ( + node.ekf_odom_flag + ), "ROSbot does not move properly in x direction. Check ekf_filter_node!" + + for node in nodes: + node.set_destination_speed(0.0, 0.0, 3.0) + + for node in nodes: + assert node.vel_stabilization_time_event.wait(timeout=60.0), ( + "The simulation is running slowly or has crashed! The time elapsed since setting" + f" the target speed is: {(node.current_time - node.goal_received_time):.1f}." + ) + assert ( + node.controller_odom_flag + ), f"{robot_name} does not rotate properly. Check rosbot_base_controller!" + assert ( + node.ekf_odom_flag + ), f"{robot_name} does not rotate properly. Check ekf_filter_node!" + + flag = node.scan_event.wait(timeout=20.0) + assert flag, f"{robot_name}'s lidar does not work properly!" + + node.destroy_node() + + finally: + rclpy.shutdown() + + # The pytest cannot kill properly the Gazebo Ignition's tasks what blocks launching + # several tests in a row. + kill_ign_linux_processes() diff --git a/rosbot_gazebo/test/test_multirobot_mecanum_simulation.py b/rosbot_gazebo/test/test_multirobot_mecanum_simulation.py new file mode 100644 index 00000000..906cb700 --- /dev/null +++ b/rosbot_gazebo/test/test_multirobot_mecanum_simulation.py @@ -0,0 +1,146 @@ +# Copyright 2021 Open Source Robotics Foundation, Inc. +# Copyright 2023 Intel Corporation. All Rights Reserved. +# Copyright 2023 Husarion +# +# 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. + +import launch_pytest +import pytest +import rclpy +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import ExecuteProcess +from launch_testing.actions import ReadyToTest +from launch_testing.util import KeepAliveProc +from rclpy.executors import MultiThreadedExecutor +from threading import Thread + +from test_utils import SimulationTestNode +from test_ign_kill_utils import kill_ign_linux_processes + +robot_names = ["robot1", "robot2", "robot3", "robot4"] + + +@launch_pytest.fixture +def generate_test_description(): + # This is necessary to get unbuffered output from the process under test + proc_env = os.environ.copy() + proc_env["PYTHONUNBUFFERED"] = "1" + + # IncludeLaunchDescription does not work with robots argument + simulation_launch = ExecuteProcess( + cmd=[ + "ros2", + "launch", + "rosbot_gazebo", + "multirobot_simulation.launch.py", + ( + f'world:={get_package_share_directory("husarion_office_gz")}' + "/worlds/empty_with_plugins.sdf" + ), + "robots:=robot1={y: 0.0}; robot2={y: 1.0}; robot3={y: 2.0}; robot4={y: 3.0}", + "headless:=True", + "mecanum:=True", + ], + output="screen", + ) + + return LaunchDescription( + [ + simulation_launch, + KeepAliveProc(), + # Tell launch to start the test + ReadyToTest(), + ] + ) + + +@pytest.mark.launch(fixture=generate_test_description) +def test_multirobot_mecanum_simulation(): + robot_names = ["robot1", "robot2", "robot3", "robot4"] + rclpy.init() + try: + nodes = [] + executor = MultiThreadedExecutor(num_threads=len(robot_names)) + + for robot_name in robot_names: + node = SimulationTestNode("test_simulation", namespace=robot_name) + node.create_test_subscribers_and_publishers() + nodes.append(node) + executor.add_node(node) + + ros_spin_thread = Thread(target=lambda executor: executor.spin(), args=(executor,)) + ros_spin_thread.start() + + for node in nodes: + # 0.9 m/s and 3.0 rad/s are controller's limits defined in + # rosbot_controller/config/mecanum_drive_controller.yaml + node.set_destination_speed(0.9, 0.0, 0.0) + + for node in nodes: + assert node.vel_stabilization_time_event.wait(timeout=120.0), ( + "The simulation is running slowly or has crashed! The time elapsed since setting" + f" the target speed is: {(node.current_time - node.goal_received_time):.1f}." + ) + assert ( + node.controller_odom_flag + ), "ROSbot does not move properly in x direction. Check rosbot_base_controller!" + assert ( + node.ekf_odom_flag + ), "ROSbot does not move properly in x direction. Check ekf_filter_node!" + + for node in nodes: + # 0.9 m/s and 3.0 rad/s are controller's limits defined in + # rosbot_controller/config/mecanum_drive_controller.yaml + node.set_destination_speed(0.0, 0.9, 0.0) + + for node in nodes: + assert node.vel_stabilization_time_event.wait(timeout=120.0), ( + "The simulation is running slowly or has crashed! The time elapsed since setting" + f" the target speed is: {(node.current_time - node.goal_received_time):.1f}." + ) + assert ( + node.controller_odom_flag + ), "ROSbot does not move properly in y direction. Check rosbot_base_controller!" + assert ( + node.ekf_odom_flag + ), "ROSbot does not move properly in y direction. Check ekf_filter_node!" + + for node in nodes: + node.set_destination_speed(0.0, 0.0, 3.0) + + for node in nodes: + assert node.vel_stabilization_time_event.wait(timeout=120.0), ( + "The simulation is running slowly or has crashed! The time elapsed since setting" + f" the target speed is: {(node.current_time - node.goal_received_time):.1f}." + ) + assert ( + node.controller_odom_flag + ), f"{robot_name} does not rotate properly. Check rosbot_base_controller!" + assert ( + node.ekf_odom_flag + ), f"{robot_name} does not rotate properly. Check ekf_filter_node!" + + flag = node.scan_event.wait(timeout=20.0) + assert flag, f"{robot_name}'s lidar does not work properly!" + + node.destroy_node() + + finally: + rclpy.shutdown() + + # The pytest cannot kill properly the Gazebo Ignition's tasks what blocks launching + # several tests in a row. + kill_ign_linux_processes() diff --git a/rosbot_gazebo/test/test_namespaced_diff_drive_simulation.py b/rosbot_gazebo/test/test_namespaced_diff_drive_simulation.py new file mode 100644 index 00000000..7a757cef --- /dev/null +++ b/rosbot_gazebo/test/test_namespaced_diff_drive_simulation.py @@ -0,0 +1,113 @@ +# Copyright 2021 Open Source Robotics Foundation, Inc. +# Copyright 2023 Intel Corporation. All Rights Reserved. +# Copyright 2023 Husarion +# +# 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. + +import launch_pytest +import pytest +import rclpy +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription +from launch.substitutions import PathJoinSubstitution +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch_testing.actions import ReadyToTest +from launch_testing.util import KeepAliveProc + +from test_utils import SimulationTestNode +from test_ign_kill_utils import kill_ign_linux_processes + + +@launch_pytest.fixture +def generate_test_description(): + # This is necessary to get unbuffered output from the process under test + proc_env = os.environ.copy() + proc_env["PYTHONUNBUFFERED"] = "1" + + rosbot_gazebo = get_package_share_directory("rosbot_gazebo") + simulation_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution( + [ + rosbot_gazebo, + "launch", + "simulation.launch.py", + ] + ) + ), + launch_arguments={ + "headless": "True", + "world": PathJoinSubstitution( + [ + get_package_share_directory("husarion_office_gz"), + "worlds", + "empty_with_plugins.sdf", + ] + ), + "namespace": "rosbot2r", + }.items(), + ) + + return LaunchDescription( + [ + simulation_launch, + KeepAliveProc(), + # Tell launch to start the test + ReadyToTest(), + ] + ) + + +@pytest.mark.launch(fixture=generate_test_description) +def test_namespaced_diff_drive_simulation(): + rclpy.init() + try: + node = SimulationTestNode("test_simulation", namespace="rosbot2r") + node.create_test_subscribers_and_publishers() + node.start_node_thread() + + # 0.9 m/s and 3.0 rad/s are controller's limits defined in + # rosbot_controller/config/diff_drive_controller.yaml + node.set_destination_speed(0.9, 0.0, 0.0) + assert node.vel_stabilization_time_event.wait(timeout=40.0), ( + "The simulation is running slowly or has crashed! The time elapsed since setting the" + f" target speed is: {(node.current_time - node.goal_received_time):.1f}." + ) + assert ( + node.controller_odom_flag + ), "ROSbot does not move properly in x direction. Check rosbot_base_controller!" + assert ( + node.ekf_odom_flag + ), "ROSbot does not move properly in x direction. Check ekf_filter_node!" + + node.set_destination_speed(0.0, 0.0, 3.0) + assert node.vel_stabilization_time_event.wait(timeout=40.0), ( + "The simulation is running slowly or has crashed! The time elapsed since setting the" + f" target speed is: {(node.current_time - node.goal_received_time):.1f}." + ) + assert ( + node.controller_odom_flag + ), "ROSbot does not rotate properly. Check rosbot_base_controller!" + assert node.ekf_odom_flag, "ROSbot does not rotate properly. Check ekf_filter_node!" + + flag = node.scan_event.wait(timeout=20.0) + assert flag, "ROSbot's lidar does not work properly!" + + finally: + # The pytest cannot kill properly the Gazebo Ignition's tasks what blocks launching + # several tests in a row. + kill_ign_linux_processes() + rclpy.shutdown() diff --git a/rosbot_gazebo/test/test_namespaced_mecanum_simulation.py b/rosbot_gazebo/test/test_namespaced_mecanum_simulation.py new file mode 100644 index 00000000..92ff2570 --- /dev/null +++ b/rosbot_gazebo/test/test_namespaced_mecanum_simulation.py @@ -0,0 +1,126 @@ +# Copyright 2021 Open Source Robotics Foundation, Inc. +# Copyright 2023 Intel Corporation. All Rights Reserved. +# Copyright 2023 Husarion +# +# 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. + +import launch_pytest +import pytest +import rclpy +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription +from launch.substitutions import PathJoinSubstitution +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch_testing.actions import ReadyToTest +from launch_testing.util import KeepAliveProc + +from test_utils import SimulationTestNode +from test_ign_kill_utils import kill_ign_linux_processes + + +@launch_pytest.fixture +def generate_test_description(): + # This is necessary to get unbuffered output from the process under test + proc_env = os.environ.copy() + proc_env["PYTHONUNBUFFERED"] = "1" + + rosbot_gazebo = get_package_share_directory("rosbot_gazebo") + simulation_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution( + [ + rosbot_gazebo, + "launch", + "simulation.launch.py", + ] + ) + ), + launch_arguments={ + "mecanum": "True", + "world": PathJoinSubstitution( + [ + get_package_share_directory("husarion_office_gz"), + "worlds", + "empty_with_plugins.sdf", + ] + ), + "headless": "True", + "namespace": "rosbot2r", + }.items(), + ) + + return LaunchDescription( + [ + simulation_launch, + KeepAliveProc(), + # Tell launch to start the test + ReadyToTest(), + ] + ) + + +@pytest.mark.launch(fixture=generate_test_description) +def test_namespaced_mecanum_simulation(): + rclpy.init() + try: + node = SimulationTestNode("test_simulation", namespace="rosbot2r") + node.create_test_subscribers_and_publishers() + node.start_node_thread() + + # 0.9 m/s and 3.0 rad/s are controller's limits defined in + # rosbot_controller/config/mecanum_drive_controller.yaml + node.set_destination_speed(0.9, 0.0, 0.0) + assert node.vel_stabilization_time_event.wait(timeout=40.0), ( + "The simulation is running slowly or has crashed! The time elapsed since setting the" + f" target speed is: {(node.current_time - node.goal_received_time):.1f}." + ) + assert ( + node.controller_odom_flag + ), "ROSbot does not move properly in x direction. Check rosbot_base_controller!" + assert ( + node.ekf_odom_flag + ), "ROSbot does not move properly in x direction. Check ekf_filter_node!" + + node.set_destination_speed(0.0, 0.9, 0.0) + assert node.vel_stabilization_time_event.wait(timeout=40.0), ( + "The simulation is running slowly or has crashed! The time elapsed since setting the" + f" target speed is: {(node.current_time - node.goal_received_time):.1f}." + ) + assert ( + node.controller_odom_flag + ), "ROSbot does not move properly in y direction. Check rosbot_base_controller!" + assert ( + node.ekf_odom_flag + ), "ROSbot does not move properly in y direction. Check ekf_filter_node!" + + node.set_destination_speed(0.0, 0.0, 3.0) + assert node.vel_stabilization_time_event.wait(timeout=40.0), ( + "The simulation is running slowly or has crashed! The time elapsed since setting the" + f" target speed is: {(node.current_time - node.goal_received_time):.1f}." + ) + assert ( + node.controller_odom_flag + ), "ROSbot does not rotate properly. Check rosbot_base_controller!" + assert node.ekf_odom_flag, "ROSbot does not rotate properly. Check ekf_filter_node!" + + flag = node.scan_event.wait(timeout=20.0) + assert flag, "ROSbot's lidar does not work properly!" + + finally: + # The pytest cannot kill properly the Gazebo Ignition's tasks what blocks launching + # several tests in a row. + kill_ign_linux_processes() + rclpy.shutdown()