[RPP] Adding support for rotate in place cusps

nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp

@@ -419,12 +419,27 @@ double RegulatedPurePursuitController::findVelocitySignChange(
     /* Checking for the existance of cusp, in the path, using the dot product
     and determine it's distance from the robot. If there is no cusp in the path,
     then just determine the distance to the goal location. */
-    if ( (oa_x * ab_x) + (oa_y * ab_y) < 0.0) {
+    const double dot_prod = (oa_x * ab_x) + (oa_y * ab_y);
+    if (dot_prod < 0.0) {
       // returning the distance if there is a cusp
       // The transformed path is in the robots frame, so robot is at the origin
       return hypot(
         transformed_plan.poses[pose_id].pose.position.x,
         transformed_plan.poses[pose_id].pose.position.y);
+    } else if (
+      (hypot(oa_x, oa_y) == 0.0 &&
+        transformed_plan.poses[pose_id - 1].pose.orientation !=
+        transformed_plan.poses[pose_id].pose.orientation)
+      ||
+      (hypot(ab_x, ab_y) == 0.0 &&
+        transformed_plan.poses[pose_id].pose.orientation !=
+        transformed_plan.poses[pose_id + 1].pose.orientation))
+    {
+      // returning the distance since the points overlap
+      // but are not simply duplicate points (e.g. in place rotation)
+      return hypot(
+        transformed_plan.poses[pose_id].pose.position.x,
+        transformed_plan.poses[pose_id].pose.position.y);
     }
   }
 

nav2_util/include/nav2_util/geometry_utils.hpp

@@ -132,7 +132,7 @@ inline Iter min_by(Iter begin, Iter end, Getter getCompareVal)
   Iter lowest_it = begin;
   for (Iter it = ++begin; it != end; ++it) {
     auto comp = getCompareVal(*it);
-    if (comp < lowest) {
+    if (comp <= lowest) {
       lowest = comp;
       lowest_it = it;
     }