Update angular velocity after constraining linear velocity (#2165)

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 <[email protected]>
ros-navigation · Feb 2, 2021 · 98d1c8c · 98d1c8c
1 parent fd3f93b
commit 98d1c8c
Showing 1 changed file with 3 additions and 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
@@ -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