Gazebo test (#86)

* Wait for velocity stabilization * Apply suggestions from code review Co-authored-by: Jakub Delicat <[email protected]> * Apply suggestions from code review * refactor * test * headless * fix sim time * update * Update rosbot_gazebo/test/test_mecanum_simulation.py Co-authored-by: Jakub Delicat <[email protected]> --------- Co-authored-by: Jakub Delicat <[email protected]>
husarion · Dec 5, 2023 · 9afd60c · 9afd60c
1 parent 98d7776
commit 9afd60c
Show file tree

Hide file tree

Showing 4 changed files with 70 additions and 42 deletions.
diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml
@@ -16,7 +16,7 @@ repos:
             args: [--pytest-test-first]
 
     - repo: https://github.com/codespell-project/codespell
-      rev: v1.16.0
+      rev: v2.2.6
       hooks:
           - id: codespell
             name: codespell
@@ -26,13 +26,13 @@ repos:
             types: [text]
 
     - repo: https://github.com/jumanjihouse/pre-commit-hook-yamlfmt
-      rev: 0.2.1
+      rev: 0.2.3
       hooks:
           - id: yamlfmt
             files: ^.github|./\.yaml
 
     - repo: https://github.com/psf/black
-      rev: 23.10.1
+      rev: 23.11.0
       hooks:
         - id: black
           args: ["--line-length=99", "--experimental-string-processing"]

diff --git a/rosbot_gazebo/test/test_diff_drive_simulation.py b/rosbot_gazebo/test/test_diff_drive_simulation.py
@@ -41,9 +41,7 @@ def generate_test_description():
                 ]
             )
         ),
-        launch_arguments={
-            "headless": "True",
-        }.items(),
+        launch_arguments={"headless": "True", "world": "empty.sdf"}.items(),
     )
 
     return LaunchDescription([simulation_launch])
@@ -65,18 +63,26 @@ 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)
-        controller_flag = node.controller_odom_event.wait(timeout=20.0)
-        ekf_flag = node.ekf_odom_event.wait(timeout=20.0)
+        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 (
-            controller_flag
+            node.controller_odom_flag
         ), "ROSbot does not move properly in x direction. Check rosbot_base_controller!"
-        assert ekf_flag, "ROSbot does not move properly in x direction. Check ekf_filter_node!"
+        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)
-        controller_flag = node.controller_odom_event.wait(timeout=20.0)
-        ekf_flag = node.ekf_odom_event.wait(timeout=20.0)
-        assert controller_flag, "ROSbot does not rotate properly. Check rosbot_base_controller!"
-        assert ekf_flag, "ROSbot does not rotate properly. Check ekf_filter_node!"
+        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!"
+        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!"

diff --git a/rosbot_gazebo/test/test_mecanum_simulation.py b/rosbot_gazebo/test/test_mecanum_simulation.py
@@ -41,10 +41,7 @@ def generate_test_description():
                 ]
             )
         ),
-        launch_arguments={
-            "mecanum": "True",
-            "headless": "True",
-        }.items(),
+        launch_arguments={"mecanum": "True", "headless": "True", "world": "empty.sdf"}.items(),
     )
 
     return LaunchDescription([simulation_launch])
@@ -66,26 +63,38 @@ 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)
-        controller_flag = node.controller_odom_event.wait(timeout=20.0)
-        ekf_flag = node.ekf_odom_event.wait(timeout=20.0)
+        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 (
-            controller_flag
+            node.controller_odom_flag
         ), "ROSbot does not move properly in x direction. Check rosbot_base_controller!"
-        assert ekf_flag, "ROSbot does not move properly in x direction. Check ekf_filter_node!"
+        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)
-        controller_flag = node.controller_odom_event.wait(timeout=20.0)
-        ekf_flag = node.ekf_odom_event.wait(timeout=20.0)
+        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 (
-            controller_flag
+            node.controller_odom_flag
         ), "ROSbot does not move properly in y direction. Check rosbot_base_controller!"
-        assert ekf_flag, "ROSbot does not move properly in y direction. Check ekf_filter_node!"
+        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)
-        controller_flag = node.controller_odom_event.wait(timeout=20.0)
-        ekf_flag = node.ekf_odom_event.wait(timeout=20.0)
-        assert controller_flag, "ROSbot does not rotate properly. Check rosbot_base_controller!"
-        assert ekf_flag, "ROSbot does not rotate properly. Check ekf_filter_node!"
+        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!"
+        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!"

diff --git a/rosbot_gazebo/test/test_utils.py b/rosbot_gazebo/test/test_utils.py
@@ -37,24 +37,35 @@ class SimulationTestNode(Node):
 
     def __init__(self, name="test_node"):
         super().__init__(name)
+
+        # Use simulation time to correct run on slow machine
+        use_sim_time = rclpy.parameter.Parameter("use_sim_time", rclpy.Parameter.Type.BOOL, True)
+        self.set_parameters([use_sim_time])
+
+        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
 
-        self.controller_odom_event = Event()
-        self.ekf_odom_event = Event()
+        self.controller_odom_flag = False
+        self.ekf_odom_flag = False
         self.odom_tf_event = Event()
         self.scan_event = Event()
 
-    def clear_odom_events(self):
-        self.controller_odom_event.clear()
-        self.ekf_odom_event.clear()
+    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)
@@ -79,9 +90,9 @@ def start_node_thread(self):
         self.timer = self.create_timer(1.0 / 10.0, self.timer_callback)
 
     def is_twist_ok(self, twist: Twist):
-        def are_close_to_each_other(true_value, dest_value, tolerance=self.ACCURACY, eps=0.01):
+        def are_close_to_each_other(current_value, dest_value, tolerance=self.ACCURACY, eps=0.01):
             acceptable_range = dest_value * tolerance
-            return abs(true_value - dest_value) <= acceptable_range + eps
+            return abs(current_value - dest_value) <= acceptable_range + eps
 
         x_ok = are_close_to_each_other(twist.linear.x, self.v_x)
         y_ok = are_close_to_each_other(twist.linear.y, self.v_y)
@@ -90,12 +101,10 @@ def are_close_to_each_other(true_value, dest_value, tolerance=self.ACCURACY, eps
         return x_ok and y_ok and yaw_ok
 
     def controller_callback(self, data: Odometry):
-        if self.is_twist_ok(data.twist.twist):
-            self.controller_odom_event.set()
+        self.controller_odom_flag = self.is_twist_ok(data.twist.twist)
 
     def ekf_callback(self, data: Odometry):
-        if self.is_twist_ok(data.twist.twist):
-            self.ekf_odom_event.set()
+        self.ekf_odom_flag = self.is_twist_ok(data.twist.twist)
 
     def lookup_transform_odom(self):
         try:
@@ -108,6 +117,10 @@ def timer_callback(self):
         self.publish_cmd_vel_messages()
         self.lookup_transform_odom()
 
+        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):
         if data.ranges:
             self.scan_event.set()