diff --git a/rosbot_gazebo/test/test_diff_drive_simulation.py b/rosbot_gazebo/test/test_diff_drive_simulation.py index 52c72b23..135735fa 100644 --- a/rosbot_gazebo/test/test_diff_drive_simulation.py +++ b/rosbot_gazebo/test/test_diff_drive_simulation.py @@ -63,7 +63,10 @@ def test_diff_drive_simulation(): # 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=20.0), "Simulation crashed!" + assert node.vel_stabilization_time_event.wait(timeout=20.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!" @@ -72,7 +75,10 @@ def test_diff_drive_simulation(): ), "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=20.0), "Simulation crashed!" + assert node.vel_stabilization_time_event.wait(timeout=20.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!" diff --git a/rosbot_gazebo/test/test_mecanum_simulation.py b/rosbot_gazebo/test/test_mecanum_simulation.py index 704829b0..e827ad80 100644 --- a/rosbot_gazebo/test/test_mecanum_simulation.py +++ b/rosbot_gazebo/test/test_mecanum_simulation.py @@ -63,7 +63,10 @@ def test_mecanum_simulation(): # 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=20.0), "Simulation crashed!" + assert node.vel_stabilization_time_event.wait(timeout=20.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!" @@ -72,7 +75,10 @@ def test_mecanum_simulation(): ), "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=20.0), "Simulation crashed!" + assert node.vel_stabilization_time_event.wait(timeout=20.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!" @@ -81,7 +87,10 @@ def test_mecanum_simulation(): ), "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=20.0), "Simulation crashed!" + assert node.vel_stabilization_time_event.wait(timeout=20.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_xl_base_controller!" diff --git a/rosbot_gazebo/test/test_utils.py b/rosbot_gazebo/test/test_utils.py index 60402b01..ed111f40 100644 --- a/rosbot_gazebo/test/test_utils.py +++ b/rosbot_gazebo/test/test_utils.py @@ -117,8 +117,8 @@ def timer_callback(self): self.publish_cmd_vel_messages() self.lookup_transform_odom() - current_time_s = 1e-9 * self.get_clock().now().nanoseconds - if current_time_s > self.goal_received_time + self.VELOCITY_STABILIZATION_DELAY: + self.current_time = 1e-9 * self.get_clock().now().nanoseconds + if self.current_time > self.goal_received_time + self.VELOCITY_STABILIZATION_DELAY: self.vel_stabilization_time_event.set() def scan_callback(self, data: LaserScan):