diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index a71dc9ba..673c0755 100644 --- 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 index 11f769f9..135735fa 100644 --- 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 index c6b7f9be..ed6a2627 100644 --- 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 index 92e2ea56..ed111f40 100644 --- 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()