diff --git a/rosbot_controller/test/test_multirobot_controllers.py b/rosbot_controller/test/test_multirobot_controllers.py index 130956cf..6e7963a7 100644 --- a/rosbot_controller/test/test_multirobot_controllers.py +++ b/rosbot_controller/test/test_multirobot_controllers.py @@ -66,17 +66,17 @@ def test_multirobot_controllers_startup_success(): node.start_publishing_fake_hardware() node.start_node_thread() - msgs_received_flag = node.joint_state_msg_event.wait(timeout=10.0) + msgs_received_flag = node.joint_state_msg_event.wait(timeout=20.0) assert msgs_received_flag, ( f"Expected JointStates message but it was not received. Check {robot_name}/" "joint_state_broadcaster!" ) - msgs_received_flag = node.odom_msg_event.wait(timeout=10.0) + msgs_received_flag = node.odom_msg_event.wait(timeout=20.0) assert msgs_received_flag, ( f"Expected Odom message but it was not received. Check {robot_name}/" "rosbot_base_controller!" ) - msgs_received_flag = node.imu_msg_event.wait(timeout=10.0) + msgs_received_flag = node.imu_msg_event.wait(timeout=20.0) assert ( msgs_received_flag ), f"Expected Imu message but it was not received. Check {robot_name}/imu_broadcaster!" diff --git a/rosbot_gazebo/test/test_multirobot_diff_drive_simulation.py b/rosbot_gazebo/test/test_multirobot_diff_drive_simulation.py index 93188bfd..ecfa7b29 100644 --- a/rosbot_gazebo/test/test_multirobot_diff_drive_simulation.py +++ b/rosbot_gazebo/test/test_multirobot_diff_drive_simulation.py @@ -22,10 +22,10 @@ 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 rclpy.executors import MultiThreadedExecutor +from threading import Thread from test_utils import SimulationTestNode from test_ign_kill_utils import kill_ign_linux_processes @@ -48,7 +48,7 @@ def generate_test_description(): 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}", + "robots:=robot1={y: -4.0}; robot2={y: 0.0}; robot3={y: 4.0};", "headless:=True", ], output="screen", @@ -69,64 +69,74 @@ def test_multirobot_simulation(): robot_names = ["robot1", "robot2", "robot3"] rclpy.init() try: - nodes = [] + 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) + nodes[robot_name] = node executor.add_node(node) ros_spin_thread = Thread(target=lambda executor: executor.spin(), args=(executor,)) ros_spin_thread.start() - for node in nodes: + for robot_name in robot_names: + node = nodes[robot_name] # 0.9 m/s and 3.0 rad/s are controller's limits defined in - # rosbot_controller/config/mecanum_drive_controller.yaml + # rosbot_controller/config/diff_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}." + for robot_name in robot_names: + node = nodes[robot_name] + assert node.vel_stabilization_time_event.wait(timeout=120.0), ( + f"{robot_name}: The simulation is running slowly or has crashed! The time elapsed" + " since setting the target speed is:" + f" {(node.current_time - node.goal_received_time):.1f}." + ) + assert node.controller_odom_flag, ( + f"{robot_name}: does not move properly in x direction. Check" + f" rosbot_base_controller! Twist: {node.twist}" + ) + assert node.ekf_odom_flag, ( + f"{robot_name}: does not move properly in x direction. Check ekf_filter_node!" + f" Twist: {node.twist}" ) - 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: + for robot_name in robot_names: + node = nodes[robot_name] 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}." + for robot_name in robot_names: + node = nodes[robot_name] + assert node.vel_stabilization_time_event.wait(timeout=120.0), ( + f"{robot_name}: The simulation is running slowly or has crashed! The time elapsed" + " since setting the target speed is:" + f" {(node.current_time - node.goal_received_time):.1f}." + ) + assert node.controller_odom_flag, ( + f"{robot_name} does not rotate properly. Check rosbot_base_controller! Twist:" + f" {node.twist}" ) - 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!" + ), f"{robot_name} does not rotate properly. Check ekf_filter_node! Twist: {node.twist}" flag = node.scan_event.wait(timeout=20.0) assert flag, f"{robot_name}'s lidar does not work properly!" for i in range(len(node.RANGE_SENSORS_TOPICS)): flag = node.ranges_events[i].wait(timeout=20.0) - assert ( - flag - ), f"ROSbot's range sensor {node.RANGE_SENSORS_TOPICS[i]} does not work properly!" + assert flag, ( + f"{robot_name}:'s range sensor {node.RANGE_SENSORS_TOPICS[i]} does not work" + " properly!" + ) flag = node.camera_color_event.wait(timeout=20.0) - assert flag, "ROSbot's camera color image does not work properly!" + assert flag, f"{robot_name}:'s camera color image does not work properly!" flag = node.camera_points_event.wait(timeout=20.0) - assert flag, "ROSbot's camera point cloud does not work properly!" + assert flag, f"{robot_name}:'s camera point cloud does not work properly!" node.destroy_node() diff --git a/rosbot_gazebo/test/test_multirobot_mecanum_simulation.py b/rosbot_gazebo/test/test_multirobot_mecanum_simulation.py index f7467c7e..f760acdd 100644 --- a/rosbot_gazebo/test/test_multirobot_mecanum_simulation.py +++ b/rosbot_gazebo/test/test_multirobot_mecanum_simulation.py @@ -48,7 +48,7 @@ def generate_test_description(): 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}", + "robots:=robot1={y: -4.0}; robot2={y: 0.0}; robot3={y: 4.0};", "headless:=True", "mecanum:=True", ], @@ -70,81 +70,96 @@ def test_multirobot_mecanum_simulation(): robot_names = ["robot1", "robot2", "robot3"] rclpy.init() try: - nodes = [] + 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) + nodes[robot_name] = node executor.add_node(node) ros_spin_thread = Thread(target=lambda executor: executor.spin(), args=(executor,)) ros_spin_thread.start() - for node in nodes: + for robot_name in robot_names: + node = nodes[robot_name] # 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: + for robot_name in robot_names: + node = nodes[robot_name] 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}." + f"{robot_name}: The simulation is running slowly or has crashed! The time elapsed" + " since setting the target speed is:" + f" {(node.current_time - node.goal_received_time):.1f}." + ) + assert node.controller_odom_flag, ( + f"{robot_name}: does not move properly in x direction. Check" + f" rosbot_base_controller! Twist: {node.twist}" + ) + assert node.ekf_odom_flag, ( + f"{robot_name}: does not move properly in x direction. Check ekf_filter_node!" + f" Twist: {node.twist}" ) - 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: + for robot_name in robot_names: + node = nodes[robot_name] # 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: + for robot_name in robot_names: + node = nodes[robot_name] 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}." + f"{robot_name}: The simulation is running slowly or has crashed! The time elapsed" + " since setting the target speed is:" + f" {(node.current_time - node.goal_received_time):.1f}." + ) + assert node.controller_odom_flag, ( + f"{robot_name} does not move properly in y direction. Check" + f" rosbot_base_controller! Twist: {node.twist}" + ) + assert node.ekf_odom_flag, ( + f"{robot_name} does not move properly in y direction. Check ekf_filter_node!" + f" Twist: {node.twist}" ) - 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: + for robot_name in robot_names: + node = nodes[robot_name] node.set_destination_speed(0.0, 0.0, 3.0) - for node in nodes: + for robot_name in robot_names: + node = nodes[robot_name] 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}." + f"{robot_name}: The simulation is running slowly or has crashed! The time elapsed" + " since setting the target speed is:" + f" {(node.current_time - node.goal_received_time):.1f}." + ) + assert node.controller_odom_flag, ( + f"{robot_name} does not rotate properly. Check rosbot_base_controller! Twist:" + f" {node.twist}" ) - 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!" + ), f"{robot_name} does not rotate properly. Check ekf_filter_node! Twist: {node.twist}" flag = node.scan_event.wait(timeout=20.0) assert flag, f"{robot_name}'s lidar does not work properly!" for i in range(len(node.RANGE_SENSORS_TOPICS)): flag = node.ranges_events[i].wait(timeout=20.0) - assert ( - flag - ), f"ROSbot's range sensor {node.RANGE_SENSORS_TOPICS[i]} does not work properly!" + assert flag, ( + f"{robot_name}:'s range sensor {node.RANGE_SENSORS_TOPICS[i]} does not work" + " properly!" + ) flag = node.camera_color_event.wait(timeout=20.0) - assert flag, "ROSbot's camera color image does not work properly!" + assert flag, f"{robot_name}:'s camera color image does not work properly!" flag = node.camera_points_event.wait(timeout=20.0) - assert flag, "ROSbot's camera point cloud does not work properly!" + assert flag, f"{robot_name}:'s camera point cloud does not work properly!" node.destroy_node() diff --git a/rosbot_gazebo/test/test_utils.py b/rosbot_gazebo/test/test_utils.py index a4a3a071..b50db1b6 100644 --- a/rosbot_gazebo/test/test_utils.py +++ b/rosbot_gazebo/test/test_utils.py @@ -49,6 +49,7 @@ def __init__(self, name="test_node", namespace=None): self.v_x = 0.0 self.v_y = 0.0 self.v_yaw = 0.0 + self.twist = None # Debug info self.controller_odom_flag = False self.ekf_odom_flag = False @@ -116,6 +117,7 @@ def are_close_to_each_other(current_value, dest_value, tolerance=self.ACCURACY, def controller_callback(self, data: Odometry): self.get_logger().debug(f"Received twist from controller: {data.twist.twist}") self.controller_odom_flag = self.is_twist_ok(data.twist.twist) + self.twist = data.twist.twist def ekf_callback(self, data: Odometry): self.get_logger().debug(f"Received twist filtered: {data.twist.twist}")