From 9979bf536f75acdc9d826df500f1ed48e6d65bc3 Mon Sep 17 00:00:00 2001 From: rafal-gorecki <126687345+rafal-gorecki@users.noreply.github.com> Date: Tue, 7 Nov 2023 12:16:49 +0100 Subject: [PATCH] Add controller odem test (#79) * Add controller odem test * divide assertion * Change to accuracy * Update rosbot_gazebo/test/test_utils.py * Simplify * Update test_utils.py * Update rosbot_gazebo/test/test_utils.py Co-authored-by: Jakub Delicat <109142865+delihus@users.noreply.github.com> --------- Co-authored-by: Jakub Delicat <109142865+delihus@users.noreply.github.com> --- .../test/test_diff_drive_simulation.py | 22 +++++--- rosbot_gazebo/test/test_mecanum_simulation.py | 30 +++++++---- rosbot_gazebo/test/test_utils.py | 50 +++++++++++-------- 3 files changed, 64 insertions(+), 38 deletions(-) diff --git a/rosbot_gazebo/test/test_diff_drive_simulation.py b/rosbot_gazebo/test/test_diff_drive_simulation.py index 34a8019c..11f769f9 100644 --- a/rosbot_gazebo/test/test_diff_drive_simulation.py +++ b/rosbot_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_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_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_gazebo/test/test_mecanum_simulation.py b/rosbot_gazebo/test/test_mecanum_simulation.py index 3ade723a..c6b7f9be 100644 --- a/rosbot_gazebo/test/test_mecanum_simulation.py +++ b/rosbot_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_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_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_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_gazebo/test/test_utils.py b/rosbot_gazebo/test/test_utils.py index ac0b7912..92e2ea56 100644 --- a/rosbot_gazebo/test/test_utils.py +++ b/rosbot_gazebo/test/test_utils.py @@ -31,8 +31,9 @@ class SimulationTestNode(Node): __test__ = False - XY_TOLERANCE = 0.05 - YAW_TOLERANCE = 0.1 + # The inaccuracies in measurement uncertainties and wheel slippage + # cause the rosbot_base_controller to determine inaccurate odometry. + ACCURACY = 0.10 # 10% accuracy def __init__(self, name="test_node"): super().__init__(name) @@ -40,19 +41,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 @@ -60,8 +59,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_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(LaserScan, "/scan", self.scan_callback, 10) @@ -75,17 +78,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: