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: