Better diagnostic in multirobot test

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!"

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()
 

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()
 

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}")