From 071d6d420a0be7cc06bc15b44b93a680e321d0e4 Mon Sep 17 00:00:00 2001 From: rafal-gorecki Date: Fri, 3 Nov 2023 14:57:24 +0100 Subject: [PATCH 1/5] Add raw odom test --- .../test/test_diff_drive_simulation.py | 22 +++++--- .../test/test_mecanum_simulation.py | 30 +++++++---- rosbot_xl_gazebo/test/test_utils.py | 51 +++++++++++-------- 3 files changed, 65 insertions(+), 38 deletions(-) diff --git a/rosbot_xl_gazebo/test/test_diff_drive_simulation.py b/rosbot_xl_gazebo/test/test_diff_drive_simulation.py index e7d344e4..b7377782 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_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_xl_gazebo/test/test_mecanum_simulation.py b/rosbot_xl_gazebo/test/test_mecanum_simulation.py index de80240e..c9ebe783 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_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_xl_gazebo/test/test_utils.py b/rosbot_xl_gazebo/test/test_utils.py index d1513f07..1f4731ac 100644 --- a/rosbot_xl_gazebo/test/test_utils.py +++ b/rosbot_xl_gazebo/test/test_utils.py @@ -33,9 +33,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_base_controller to determine inaccurate odometry. + YAW_TOLERANCE = 0.2 def __init__(self, name="test_node"): super().__init__(name) @@ -43,19 +43,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 +61,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( @@ -80,17 +82,26 @@ 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): + x_ok, y_ok, yaw_ok = False, False, False if abs(twist.linear.x - self.v_x) < self.XY_TOLERANCE: - self.goal_x_event.set() + x_ok = True if abs(twist.linear.y - self.v_y) < self.XY_TOLERANCE: - self.goal_y_event.set() + y_ok = True if abs(twist.angular.z - self.v_yaw) < self.YAW_TOLERANCE: - self.goal_yaw_event.set() + yaw_ok = True + + 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() + + 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 +117,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() From 590fcb1605bd520a692bd39b3da72db2b13701e1 Mon Sep 17 00:00:00 2001 From: rafal-gorecki Date: Fri, 3 Nov 2023 18:36:37 +0100 Subject: [PATCH 2/5] Use accuracy --- .../test/test_diff_drive_simulation.py | 4 +-- .../test/test_mecanum_simulation.py | 6 ++--- rosbot_xl_gazebo/test/test_utils.py | 27 ++++++++++++++----- 3 files changed, 25 insertions(+), 12 deletions(-) diff --git a/rosbot_xl_gazebo/test/test_diff_drive_simulation.py b/rosbot_xl_gazebo/test/test_diff_drive_simulation.py index b7377782..b9cd2b79 100644 --- a/rosbot_xl_gazebo/test/test_diff_drive_simulation.py +++ b/rosbot_xl_gazebo/test/test_diff_drive_simulation.py @@ -69,13 +69,13 @@ def test_diff_drive_simulation(): 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!" + ), "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) 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 controller_flag, "ROSbot does not rotate properly. Check rosbot_xl_base_controller!" assert ekf_flag, "ROSbot does not rotate properly. Check ekf_filter_node!" flag = node.scan_event.wait(timeout=20.0) diff --git a/rosbot_xl_gazebo/test/test_mecanum_simulation.py b/rosbot_xl_gazebo/test/test_mecanum_simulation.py index c9ebe783..00e24473 100644 --- a/rosbot_xl_gazebo/test/test_mecanum_simulation.py +++ b/rosbot_xl_gazebo/test/test_mecanum_simulation.py @@ -70,7 +70,7 @@ def test_mecanum_simulation(): 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!" + ), "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) @@ -78,13 +78,13 @@ def test_mecanum_simulation(): 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!" + ), "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) 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 controller_flag, "ROSbot does not rotate properly. Check rosbot_xl_base_controller!" assert ekf_flag, "ROSbot does not rotate properly. Check ekf_filter_node!" flag = node.scan_event.wait(timeout=20.0) diff --git a/rosbot_xl_gazebo/test/test_utils.py b/rosbot_xl_gazebo/test/test_utils.py index 1f4731ac..b46ca0bc 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 # The inaccuracies in measurement uncertainties and wheel slippage - # cause the rosbot_base_controller to determine inaccurate odometry. - YAW_TOLERANCE = 0.2 + # cause the rosbot_xl_base_controller to determine inaccurate odometry. + ACCURACY = 0.20 # 20% accuracy def __init__(self, name="test_node"): super().__init__(name) @@ -62,7 +61,7 @@ def create_test_subscribers_and_publishers(self): self.cmd_vel_publisher = self.create_publisher(Twist, "cmd_vel", 10) self.controller_odom_sub = self.create_subscription( - Odometry, "/rosbot_base_controller/odom", self.controller_callback, 10 + Odometry, "/rosbot_xl_base_controller/odom", self.controller_callback, 10 ) self.ekf_odom_sub = self.create_subscription( @@ -84,13 +83,27 @@ def start_node_thread(self): def is_twist_ok(self, twist: Twist): x_ok, y_ok, yaw_ok = False, False, False - if abs(twist.linear.x - self.v_x) < self.XY_TOLERANCE: + + def are_close_to_zero(value1, value2, eps=0.05): + return abs(value1) < eps and abs(value2) < eps + + def are_close_to_each_other(true_value, dest_value, tolerance=self.ACCURACY): + acceptable_range = true_value * (tolerance / 100) + return abs(true_value - dest_value) <= acceptable_range + + if (are_close_to_zero(self.v_x, twist.linear.x)) or are_close_to_each_other( + twist.linear.x, self.v_x + ): x_ok = True - if abs(twist.linear.y - self.v_y) < self.XY_TOLERANCE: + if are_close_to_zero(self.v_y, twist.linear.y) or are_close_to_each_other( + twist.linear.y, self.v_y + ): y_ok = True - if abs(twist.angular.z - self.v_yaw) < self.YAW_TOLERANCE: + if are_close_to_zero(self.v_yaw, twist.angular.z) or are_close_to_each_other( + twist.angular.z, self.v_yaw + ): yaw_ok = True return x_ok and y_ok and yaw_ok From 2f6cd27fada9458a884e7fb9f0d3c84af86bf006 Mon Sep 17 00:00:00 2001 From: rafal-gorecki Date: Fri, 3 Nov 2023 18:39:39 +0100 Subject: [PATCH 3/5] typos --- rosbot_xl_gazebo/test/test_utils.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rosbot_xl_gazebo/test/test_utils.py b/rosbot_xl_gazebo/test/test_utils.py index b46ca0bc..f49ac8ab 100644 --- a/rosbot_xl_gazebo/test/test_utils.py +++ b/rosbot_xl_gazebo/test/test_utils.py @@ -88,7 +88,7 @@ def are_close_to_zero(value1, value2, eps=0.05): return abs(value1) < eps and abs(value2) < eps def are_close_to_each_other(true_value, dest_value, tolerance=self.ACCURACY): - acceptable_range = true_value * (tolerance / 100) + acceptable_range = dest_value * tolerance return abs(true_value - dest_value) <= acceptable_range if (are_close_to_zero(self.v_x, twist.linear.x)) or are_close_to_each_other( From 574b463034e28066ca7590dd463af283dfb412e6 Mon Sep 17 00:00:00 2001 From: rafal-gorecki Date: Fri, 3 Nov 2023 19:02:58 +0100 Subject: [PATCH 4/5] simplified --- rosbot_xl_gazebo/test/test_utils.py | 19 +++++-------------- 1 file changed, 5 insertions(+), 14 deletions(-) diff --git a/rosbot_xl_gazebo/test/test_utils.py b/rosbot_xl_gazebo/test/test_utils.py index f49ac8ab..d46dd441 100644 --- a/rosbot_xl_gazebo/test/test_utils.py +++ b/rosbot_xl_gazebo/test/test_utils.py @@ -84,26 +84,17 @@ def start_node_thread(self): def is_twist_ok(self, twist: Twist): x_ok, y_ok, yaw_ok = False, False, False - def are_close_to_zero(value1, value2, eps=0.05): - return abs(value1) < eps and abs(value2) < eps - - def are_close_to_each_other(true_value, dest_value, tolerance=self.ACCURACY): + 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 + return abs(true_value - dest_value) <= acceptable_range + eps - if (are_close_to_zero(self.v_x, twist.linear.x)) or are_close_to_each_other( - twist.linear.x, self.v_x - ): + if are_close_to_each_other(twist.linear.x, self.v_x): x_ok = True - if are_close_to_zero(self.v_y, twist.linear.y) or are_close_to_each_other( - twist.linear.y, self.v_y - ): + if are_close_to_each_other(twist.linear.y, self.v_y): y_ok = True - if are_close_to_zero(self.v_yaw, twist.angular.z) or are_close_to_each_other( - twist.angular.z, self.v_yaw - ): + if are_close_to_each_other(twist.angular.z, self.v_yaw): yaw_ok = True return x_ok and y_ok and yaw_ok From 8e2477bbe086d07fd27fd0738014b1a526eb2014 Mon Sep 17 00:00:00 2001 From: rafal-gorecki Date: Tue, 7 Nov 2023 11:57:33 +0100 Subject: [PATCH 5/5] Add suggestions --- rosbot_xl_gazebo/test/test_utils.py | 13 +++---------- 1 file changed, 3 insertions(+), 10 deletions(-) diff --git a/rosbot_xl_gazebo/test/test_utils.py b/rosbot_xl_gazebo/test/test_utils.py index d46dd441..8ea6cacb 100644 --- a/rosbot_xl_gazebo/test/test_utils.py +++ b/rosbot_xl_gazebo/test/test_utils.py @@ -82,20 +82,13 @@ def start_node_thread(self): self.timer = self.create_timer(1.0 / 10.0, self.timer_callback) def is_twist_ok(self, twist: Twist): - x_ok, y_ok, yaw_ok = False, False, False - 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 - if are_close_to_each_other(twist.linear.x, self.v_x): - x_ok = True - - if are_close_to_each_other(twist.linear.y, self.v_y): - y_ok = True - - if are_close_to_each_other(twist.angular.z, self.v_yaw): - yaw_ok = True + 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) return x_ok and y_ok and yaw_ok