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 5fdfd844ad5..b5550162349 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 @@ -516,6 +516,20 @@ double RegulatedPurePursuitController::findVelocitySignChange( 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); } if (