Merge pull request #54 from husarion/add-controller-odom-test

Add raw odom test

rosbot_xl_gazebo/test/test_diff_drive_simulation.py

@@ -57,23 +57,29 @@ def test_diff_drive_simulation():
         node.create_test_subscribers_and_publishers()
         node.start_node_thread()
 
-        msgs_received_flag = node.odom_tf_event.wait(timeout=60.0)
+        flag = node.odom_tf_event.wait(timeout=60.0)
         assert (
-            msgs_received_flag
+            flag
         ), "Expected odom to base_link tf but it was not received. Check robot_localization!"
 
         # 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)
-        msgs_received_flag = node.goal_x_event.wait(timeout=60.0)
-        assert msgs_received_flag, "ROSbot does not move properly in x direction!"
+        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 move properly in x direction. Check rosbot_xl_base_controller!"
+        assert ekf_flag, "ROSbot does not move properly in x direction. Check ekf_filter_node!"
 
         node.set_destination_speed(0.0, 0.0, 3.0)
-        msgs_received_flag = node.goal_yaw_event.wait(timeout=60.0)
-        assert msgs_received_flag, "ROSbot does not rotate properly!"
+        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_xl_base_controller!"
+        assert ekf_flag, "ROSbot does not rotate properly. Check ekf_filter_node!"
 
-        msgs_received_flag = node.scan_event.wait(timeout=60.0)
-        assert msgs_received_flag, "ROSbot's lidar does not work properly!"
+        flag = node.scan_event.wait(timeout=20.0)
+        assert flag, "ROSbot's lidar does not work properly!"
 
     finally:
         # The pytest cannot kill properly the Gazebo Ignition's tasks what blocks launching

rosbot_xl_gazebo/test/test_mecanum_simulation.py

@@ -58,27 +58,37 @@ def test_mecanum_simulation():
         node.create_test_subscribers_and_publishers()
         node.start_node_thread()
 
-        msgs_received_flag = node.odom_tf_event.wait(timeout=60.0)
+        flag = node.odom_tf_event.wait(timeout=60.0)
         assert (
-            msgs_received_flag
+            flag
         ), "Expected odom to base_link tf but it was not received. Check robot_localization!"
 
         # 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)
-        msgs_received_flag = node.goal_x_event.wait(timeout=60.0)
-        assert msgs_received_flag, "ROSbot does not move properly in x direction!"
+        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 move properly in x direction. Check rosbot_xl_base_controller!"
+        assert ekf_flag, "ROSbot does not move properly in x direction. Check ekf_filter_node!"
 
         node.set_destination_speed(0.0, 0.9, 0.0)
-        msgs_received_flag = node.goal_y_event.wait(timeout=60.0)
-        assert msgs_received_flag, "ROSbot does not move properly in y direction!"
+        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 move properly in y direction. Check rosbot_xl_base_controller!"
+        assert ekf_flag, "ROSbot does not move properly in y direction. Check ekf_filter_node!"
 
         node.set_destination_speed(0.0, 0.0, 3.0)
-        msgs_received_flag = node.goal_yaw_event.wait(timeout=60.0)
-        assert msgs_received_flag, "ROSbot does not rotate properly!"
+        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_xl_base_controller!"
+        assert ekf_flag, "ROSbot does not rotate properly. Check ekf_filter_node!"
 
-        msgs_received_flag = node.scan_event.wait(timeout=60.0)
-        assert msgs_received_flag, "ROSbot's lidar does not work properly!"
+        flag = node.scan_event.wait(timeout=20.0)
+        assert flag, "ROSbot's lidar does not work properly!"
 
     finally:
         # The pytest cannot kill properly the Gazebo Ignition's tasks what blocks launching

rosbot_xl_gazebo/test/test_utils.py

@@ -32,39 +32,40 @@
 
 class SimulationTestNode(Node):
     __test__ = False
-    XY_TOLERANCE = 0.05
-    # ROSbot XL diff drive wheels are very slippery and cannot rotate robot
-    # with 3 rad/s mecanum wheels can pass tests with 0.1 tolerance
-    YAW_TOLERANCE = 0.5
+    # The inaccuracies in measurement uncertainties and wheel slippage
+    # cause the rosbot_xl_base_controller to determine inaccurate odometry.
+    ACCURACY = 0.20  # 20% accuracy
 
     def __init__(self, name="test_node"):
         super().__init__(name)
         self.v_x = 0.0
         self.v_y = 0.0
         self.v_yaw = 0.0
 
-        self.goal_x_event = Event()
-        self.goal_y_event = Event()
-        self.goal_yaw_event = Event()
+        self.controller_odom_event = Event()
+        self.ekf_odom_event = Event()
         self.odom_tf_event = Event()
         self.scan_event = Event()
 
-    def clear_events(self):
-        self.goal_x_event.clear()
-        self.goal_y_event.clear()
-        self.goal_yaw_event.clear()
+    def clear_odom_events(self):
+        self.controller_odom_event.clear()
+        self.ekf_odom_event.clear()
 
     def set_destination_speed(self, v_x, v_y, v_yaw):
-        self.clear_events()
+        self.clear_odom_events()
         self.v_x = v_x
         self.v_y = v_y
         self.v_yaw = v_yaw
 
     def create_test_subscribers_and_publishers(self):
         self.cmd_vel_publisher = self.create_publisher(Twist, "cmd_vel", 10)
 
-        self.odom_sub = self.create_subscription(
-            Odometry, "/odometry/filtered", self.odometry_callback, 10
+        self.controller_odom_sub = self.create_subscription(
+            Odometry, "/rosbot_xl_base_controller/odom", self.controller_callback, 10
+        )
+
+        self.ekf_odom_sub = self.create_subscription(
+            Odometry, "/odometry/filtered", self.ekf_callback, 10
         )
 
         self.scan_sub = self.create_subscription(
@@ -80,17 +81,24 @@ def start_node_thread(self):
         self.ros_spin_thread.start()
         self.timer = self.create_timer(1.0 / 10.0, self.timer_callback)
 
-    def odometry_callback(self, data: Odometry):
-        twist = data.twist.twist
+    def is_twist_ok(self, twist: Twist):
+        def are_close_to_each_other(true_value, dest_value, tolerance=self.ACCURACY, eps=0.01):
+            acceptable_range = dest_value * tolerance
+            return abs(true_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)
+        yaw_ok = are_close_to_each_other(twist.angular.z, self.v_yaw)
 
-        if abs(twist.linear.x - self.v_x) < self.XY_TOLERANCE:
-            self.goal_x_event.set()
+        return x_ok and y_ok and yaw_ok
 
-        if abs(twist.linear.y - self.v_y) < self.XY_TOLERANCE:
-            self.goal_y_event.set()
+    def controller_callback(self, data: Odometry):
+        if self.is_twist_ok(data.twist.twist):
+            self.controller_odom_event.set()
 
-        if abs(twist.angular.z - self.v_yaw) < self.YAW_TOLERANCE:
-            self.goal_yaw_event.set()
+    def ekf_callback(self, data: Odometry):
+        if self.is_twist_ok(data.twist.twist):
+            self.ekf_odom_event.set()
 
     def lookup_transform_odom(self):
         try:
@@ -106,7 +114,7 @@ def timer_callback(self):
     def scan_callback(self, data: LaserScan):
         for range in data.ranges:
             # minimal distance and nan configured in rosbot_xl_bringup/config_laser_filter.yaml
-            if range < 0.145 and range > 0.0 and not math.isnan(range):
+            if range < 0.145 and not math.isnan(range):
                 return
 
         self.scan_event.set()