adding support for rotate in place cusps (ros-navigation#3934)

Marc-Morcos · Jul 4, 2024 · d7ca038 · d7ca038
1 parent b557af3
commit d7ca038
Showing 1 changed file with 14 additions and 0 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
@@ -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 (