Fix oscilation following the path

controllers/easynav_regulated_pp_controller/README.md

@@ -90,7 +90,7 @@ EasyNav's design differs from Nav2's `controller_server` in ways that require so
 | `use_velocity_scaled_lookahead_dist` | Use velocity-scaled lookahead distance instead of the fixed `lookahead_dist`. |
 | `rotate_to_heading_angular_vel` | Angular velocity used while rotating in place. |
 | `use_rotate_to_heading` | Enable rotate-in-place behaviors (rough path heading and final goal heading). |
-| `rotate_to_heading_min_angle` | Angle to the carrot beyond which the robot rotates in place first. |
+| `rotate_to_heading_min_angle` | Angle to the carrot beyond which the robot rotates in place first. Hysteretic: leaving rotate-in-place mode requires the angle to drop to *half* this value (well-aligned), not just back under it, to avoid chattering between rotate-in-place and curve-follow mode near sharp turns where the carrot itself is geometrically unstable tick to tick. |
 | `use_regulated_linear_velocity_scaling` | Enable curvature-based velocity regulation. |
 | `regulated_linear_scaling_min_radius` | Turning radius below which curvature regulation kicks in. |
 | `regulated_linear_scaling_min_speed` | Minimum velocity kept under regulation. |

controllers/easynav_regulated_pp_controller/include/easynav_regulated_pp_controller/RegulatedPurePursuitController.hpp

@@ -166,7 +166,15 @@ class RegulatedPurePursuitController : public ControllerMethodBase
     double robot_yaw);
 
   /// \brief Whether the robot should rotate in place towards \p angle_to_path.
-  bool shouldRotateToPath(double angle_to_path) const;
+  /// \param angle_to_path Current angle (rad) from the robot's heading to the path.
+  /// \param currently_rotating Whether the previous tick was already rotating in place.
+  ///   Hysteretic on \p currently_rotating: entering rotate-in-place mode requires
+  ///   \p angle_to_path to exceed \ref rotate_to_heading_min_angle_, but once in it, leaving
+  ///   requires dropping to half that value (well-aligned, not merely back under the entry
+  ///   threshold) -- otherwise a noisy angle hovering near the boundary (as it reliably does at
+  ///   a sharp turn, where the lookahead carrot itself is geometrically unstable tick to tick)
+  ///   chatters the controller between rotate-in-place and curve-follow mode every tick.
+  bool shouldRotateToPath(double angle_to_path, bool currently_rotating) const;
 
   /// \brief Computes a kinematically-feasible rotate-in-place command towards \p angle_to_target.
   void rotateToHeading(

controllers/easynav_regulated_pp_controller/src/easynav_regulated_pp_controller/RegulatedPurePursuitController.cpp

@@ -277,9 +277,14 @@ RegulatedPurePursuitController::toRobotFrame(
 }
 
 bool
-RegulatedPurePursuitController::shouldRotateToPath(double angle_to_path) const
+RegulatedPurePursuitController::shouldRotateToPath(
+  double angle_to_path, bool currently_rotating) const
 {
-  return use_rotate_to_heading_ && std::fabs(angle_to_path) > rotate_to_heading_min_angle_;
+  if (!use_rotate_to_heading_) {return false;}
+  const double threshold = currently_rotating ?
+    0.5 * rotate_to_heading_min_angle_ :
+    rotate_to_heading_min_angle_;
+  return std::fabs(angle_to_path) > threshold;
 }
 
 void
@@ -489,7 +494,7 @@ RegulatedPurePursuitController::update_rt(NavState & nav_state)
     const double regulation_curvature = heuristics::calculateCurvature(
       curvature_local.x, curvature_local.y);
 
-    if (shouldRotateToPath(angle_to_path)) {
+    if (shouldRotateToPath(angle_to_path, is_rotating_to_heading_)) {
       is_rotating_to_heading_ = true;
       rotateToHeading(linear_vel, angular_vel, angle_to_path, dt);
     } else {

controllers/easynav_regulated_pp_controller/tests/regulated_pp_controller_tests.cpp

@@ -132,6 +132,13 @@ class FriendRegulatedPurePursuitController : public easynav::RegulatedPurePursui
   using easynav::RegulatedPurePursuitController::toRobotFrame;
   using easynav::RegulatedPurePursuitController::remainingPathDistance;
   using easynav::RegulatedPurePursuitController::findClosestPoseIndex;
+  using easynav::RegulatedPurePursuitController::shouldRotateToPath;
+
+  void setRotateToHeadingParams(bool use_rotate_to_heading, double min_angle)
+  {
+    use_rotate_to_heading_ = use_rotate_to_heading;
+    rotate_to_heading_min_angle_ = min_angle;
+  }
 };
 
 TEST(RegulatedPurePursuitControllerHelpers, LookAheadPointInterpolatesOnSegment)
@@ -214,6 +221,33 @@ TEST(RegulatedPurePursuitControllerHelpers, LookAheadPointClampsToPathEnd)
   EXPECT_NEAR(carrot.x, 1.0, 1e-6);
 }
 
+TEST(RegulatedPurePursuitControllerHelpers, ShouldRotateToPathHasHysteresis)
+{
+  // Regression test for the oscillation reported near sharp turns exiting an inflated area: a
+  // single fixed threshold, re-evaluated fresh every tick with no memory of the previous
+  // decision, let the controller chatter between rotate-in-place and curve-follow mode whenever
+  // the (geometrically unstable, near a sharp corner) angle-to-path hovered near it. The fix
+  // requires a *smaller* angle to leave rotate-in-place mode than the one that entered it.
+  FriendRegulatedPurePursuitController controller;
+  controller.setRotateToHeadingParams(/*use_rotate_to_heading=*/true, /*min_angle=*/0.785);
+
+  // Below the entry threshold, and not currently rotating: stay in curve-follow mode.
+  EXPECT_FALSE(controller.shouldRotateToPath(0.5, /*currently_rotating=*/false));
+  // Above the entry threshold: start rotating in place.
+  EXPECT_TRUE(controller.shouldRotateToPath(0.9, /*currently_rotating=*/false));
+
+  // The crux of the fix: once rotating, an angle that would never have *started* a rotation
+  // (0.5 < 0.785) must not end one already in progress -- the old single-threshold code would
+  // have flipped back to curve-follow mode here, which is exactly the observed oscillation.
+  EXPECT_TRUE(controller.shouldRotateToPath(0.5, /*currently_rotating=*/true));
+  // Only once well-aligned (below half the entry threshold) does it stop rotating.
+  EXPECT_FALSE(controller.shouldRotateToPath(0.3, /*currently_rotating=*/true));
+
+  // Disabled outright regardless of angle or state.
+  controller.setRotateToHeadingParams(/*use_rotate_to_heading=*/false, /*min_angle=*/0.785);
+  EXPECT_FALSE(controller.shouldRotateToPath(3.0, /*currently_rotating=*/true));
+}
+
 TEST(RegulatedPurePursuitControllerHelpers, ToRobotFrameRotatesAndTranslates)
 {
   geometry_msgs::msg::Point global_point;