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 8c332d4969e..9434b105f4c 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
@@ -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);
     }
   }
 
diff --git a/nav2_util/include/nav2_util/geometry_utils.hpp b/nav2_util/include/nav2_util/geometry_utils.hpp
index 884b7b366ed..67cc87243d6 100644
--- a/nav2_util/include/nav2_util/geometry_utils.hpp
+++ b/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;
     }