From bcaa1a1a4e67be32b9f54f8df4366699fe4f9a2b Mon Sep 17 00:00:00 2001 From: UnladenCoconut Date: Mon, 9 Nov 2026 10:37:06 +0000 Subject: [PATCH] Fix MoveStraightIn2DActionServer to stop circling the destination point The previous implementation always moved the maximum possible delta x/y, so if the the target wasnt a integer multiple of this it would circle that point when it gets close. --- .../move_straight_in_2d_action_server_node.py | 20 +++++++++++++------ 1 file changed, 14 insertions(+), 6 deletions(-) diff --git a/ros2_tutorial_workspace/src/python_package_that_uses_the_actions/python_package_that_uses_the_actions/move_straight_in_2d_action_server_node.py b/ros2_tutorial_workspace/src/python_package_that_uses_the_actions/python_package_that_uses_the_actions/move_straight_in_2d_action_server_node.py index 13ebc12..03b285d 100644 --- a/ros2_tutorial_workspace/src/python_package_that_uses_the_actions/python_package_that_uses_the_actions/move_straight_in_2d_action_server_node.py +++ b/ros2_tutorial_workspace/src/python_package_that_uses_the_actions/python_package_that_uses_the_actions/move_straight_in_2d_action_server_node.py @@ -74,12 +74,20 @@ def move_the_object_with_velocity(self, desired_position: Point, desired_speed: if distance < 0.01: return - x_direction = (self.current_position.x - desired_position.x) / distance - y_direction = (self.current_position.y - desired_position.y) / distance - - # Apply new position based on the desired velocity and direction - self.current_position.x -= x_direction * desired_speed * self.sampling_time - self.current_position.y -= y_direction * desired_speed * self.sampling_time + desired_delta_x = (desired_position.x - self.current_position.x) + desired_delta_y = (desired_position.y - self.current_position.y) + + x_direction = desired_delta_x / distance + y_direction = desired_delta_y / distance + + # maximum distance we can move in x and y based on the desired velocity, direction, and time step. + max_delta_x = x_direction * desired_speed * self.sampling_time + max_delta_y = y_direction * desired_speed * self.sampling_time + + #move either the maximum distance in x and y at the current speed, + #or just the desired move distance if we can reach it within the current time step. + self.current_position.x += min(desired_delta_x, max_delta_x) + self.current_position.y += min(desired_delta_y, max_delta_y) def execute_callback(self, goal: ServerGoalHandle) -> MoveStraightIn2D.Result: