From a217e4e96682bb60473617d6259d1010ed29c558 Mon Sep 17 00:00:00 2001 From: Alberto Tudela Date: Tue, 6 Feb 2024 17:52:51 +0100 Subject: [PATCH] Fix backward motion Signed-off-by: Alberto Tudela --- nav2_graceful_motion_controller/src/smooth_control_law.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/nav2_graceful_motion_controller/src/smooth_control_law.cpp b/nav2_graceful_motion_controller/src/smooth_control_law.cpp index c88b5afc960..ee5a38ea873 100644 --- a/nav2_graceful_motion_controller/src/smooth_control_law.cpp +++ b/nav2_graceful_motion_controller/src/smooth_control_law.cpp @@ -71,6 +71,9 @@ geometry_msgs::msg::Twist SmoothControlLaw::calculateRegularVelocity( // turning motion when the curvature is very high v = std::clamp(v, v_linear_min_, v_linear_max_); + // Set the velocity to negative if the robot is moving backwards + v = backward ? -v : v; + // Compute the angular velocity double w = curvature * v; // Bound angular velocity between [-max_angular_vel, max_angular_vel]