diff --git a/rosbot_gazebo/test/test_diff_drive_simulation.py b/rosbot_gazebo/test/test_diff_drive_simulation.py
index 2bb1be44..6a5226fb 100644
--- a/rosbot_gazebo/test/test_diff_drive_simulation.py
+++ b/rosbot_gazebo/test/test_diff_drive_simulation.py
@@ -17,7 +17,6 @@
 import launch_pytest
 import pytest
 import rclpy
-import time
 
 from ament_index_python.packages import get_package_share_directory
 from launch import LaunchDescription
@@ -42,7 +41,7 @@ def generate_test_description():
                 ]
             )
         ),
-        launch_arguments={"headless": "True", "world": "empty.sdf"}.items(),
+        launch_arguments={"headless": "False", "world": "empty.sdf"}.items(),
     )
 
     return LaunchDescription([simulation_launch])
@@ -64,7 +63,7 @@ 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)
-        time.sleep(3)  # Wait 3 seconds for the velocity ​​to stabilize
+        assert node.vel_stabilization_time_event.wait(timeout=20.0), "Simulation crashed!"
         assert (
             node.controller_odom_flag
         ), "ROSbot does not move properly in x direction. Check rosbot_base_controller!"
@@ -73,7 +72,7 @@ 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)
-        time.sleep(3)  # Wait 3 seconds for the velocity ​​to stabilize
+        assert node.vel_stabilization_time_event.wait(timeout=20.0), "Simulation crashed!"
         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 14a526a6..704829b0 100644
--- a/rosbot_gazebo/test/test_mecanum_simulation.py
+++ b/rosbot_gazebo/test/test_mecanum_simulation.py
@@ -17,7 +17,6 @@
 import launch_pytest
 import pytest
 import rclpy
-import time
 
 from ament_index_python.packages import get_package_share_directory
 from launch import LaunchDescription
@@ -64,7 +63,7 @@ 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)
-        time.sleep(3)  # Wait 3 seconds for the velocity ​​to stabilize
+        assert node.vel_stabilization_time_event.wait(timeout=20.0), "Simulation crashed!"
         assert (
             node.controller_odom_flag
         ), "ROSbot does not move properly in x direction. Check rosbot_base_controller!"
@@ -73,7 +72,7 @@ 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)
-        time.sleep(3)  # Wait 3 seconds for the velocity ​​to stabilize
+        assert node.vel_stabilization_time_event.wait(timeout=20.0), "Simulation crashed!"
         assert (
             node.controller_odom_flag
         ), "ROSbot does not move properly in y direction. Check rosbot_base_controller!"
@@ -82,7 +81,7 @@ 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)
-        time.sleep(3)  # Wait 3 seconds for the velocity ​​to stabilize
+        assert node.vel_stabilization_time_event.wait(timeout=20.0), "Simulation crashed!"
         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 72b64d6d..6e331275 100644
--- a/rosbot_gazebo/test/test_utils.py
+++ b/rosbot_gazebo/test/test_utils.py
@@ -37,6 +37,11 @@ class SimulationTestNode(Node):
 
     def __init__(self, name="test_node"):
         super().__init__(name)
+
+        self.VELOCITY_STABILIZATION_DELAY = 3
+        self.goal_received_time = 1e-9 * self.get_clock().now().nanoseconds
+        self.vel_stabilization_time_event = Event()
+
         self.v_x = 0.0
         self.v_y = 0.0
         self.v_yaw = 0.0
@@ -46,15 +51,17 @@ def __init__(self, name="test_node"):
         self.odom_tf_event = Event()
         self.scan_event = Event()
 
-    def clear_odom_events(self):
+    def clear_odom_flag(self):
         self.controller_odom_flag = False
         self.ekf_odom_flag = False
 
     def set_destination_speed(self, v_x, v_y, v_yaw):
-        self.clear_odom_events()
+        self.clear_odom_flag()
         self.v_x = v_x
         self.v_y = v_y
         self.v_yaw = v_yaw
+        self.goal_received_time = 1e-9 * self.get_clock().now().nanoseconds
+        self.vel_stabilization_time_event.clear()
 
     def create_test_subscribers_and_publishers(self):
         self.cmd_vel_publisher = self.create_publisher(Twist, "cmd_vel", 10)
@@ -106,6 +113,10 @@ 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.vel_stabilization_time_event.set()
+
     def scan_callback(self, data: LaserScan):
         if data.ranges:
             self.scan_event.set()