diff --git a/rosbot_xl_gazebo/test/test_diff_drive_simulation.py b/rosbot_xl_gazebo/test/test_diff_drive_simulation.py index e7d344e4..b9cd2b79 100644 --- a/rosbot_xl_gazebo/test/test_diff_drive_simulation.py +++ b/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 diff --git a/rosbot_xl_gazebo/test/test_mecanum_simulation.py b/rosbot_xl_gazebo/test/test_mecanum_simulation.py index de80240e..00e24473 100644 --- a/rosbot_xl_gazebo/test/test_mecanum_simulation.py +++ b/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 diff --git a/rosbot_xl_gazebo/test/test_utils.py b/rosbot_xl_gazebo/test/test_utils.py index d1513f07..8ea6cacb 100644 --- a/rosbot_xl_gazebo/test/test_utils.py +++ b/rosbot_xl_gazebo/test/test_utils.py @@ -32,10 +32,9 @@ 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) @@ -43,19 +42,17 @@ def __init__(self, name="test_node"): 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 @@ -63,8 +60,12 @@ def set_destination_speed(self, v_x, v_y, 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()