From 692f91c8ca920073679fe744791f416b5d193387 Mon Sep 17 00:00:00 2001 From: Shrijit Singh Date: Wed, 3 Feb 2021 00:20:47 +0530 Subject: [PATCH] Update angular velocity after constraining linear velocity This ensures the robot moves towards the lookahead point more closely. If the angular velocity is not updated, then the robot tries to take cuts while turning, which could lead to collisions when near obstacles Signed-off-by: Shrijit Singh --- .../src/regulated_pure_pursuit_controller.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp b/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp index 1e9f364efb..ca095cc629 100644 --- a/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp +++ b/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp @@ -235,9 +235,7 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity curvature = 2.0 * carrot_pose.pose.position.y / carrot_dist2; } - // Apply curvature to angular velocity linear_vel = desired_linear_vel_; - angular_vel = desired_linear_vel_ * curvature; // Make sure we're in compliance with basic constraints double angle_to_heading; @@ -251,6 +249,9 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity fabs(lookahead_dist - sqrt(carrot_dist2)), lookahead_dist, curvature, speed, costAtPose(pose.pose.position.x, pose.pose.position.y), linear_vel); + + // Apply curvature to angular velocity after constraining linear velocity + angular_vel = linear_vel * curvature; } // Collision checking on this velocity heading