From b766ff8420f649f4406c016c383d1cf19405acd6 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Thu, 13 Aug 2026 12:11:56 +0200 Subject: [PATCH 1/2] Fix oscilation following the path MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- .../easynav_regulated_pp_controller/README.md | 2 +- .../RegulatedPurePursuitController.hpp | 2 +- .../RegulatedPurePursuitController.cpp | 11 ++++-- .../tests/regulated_pp_controller_tests.cpp | 34 +++++++++++++++++++ 4 files changed, 44 insertions(+), 5 deletions(-) diff --git a/controllers/easynav_regulated_pp_controller/README.md b/controllers/easynav_regulated_pp_controller/README.md index 03be5d46..7eb5f77d 100644 --- a/controllers/easynav_regulated_pp_controller/README.md +++ b/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. | diff --git a/controllers/easynav_regulated_pp_controller/include/easynav_regulated_pp_controller/RegulatedPurePursuitController.hpp b/controllers/easynav_regulated_pp_controller/include/easynav_regulated_pp_controller/RegulatedPurePursuitController.hpp index 607e2075..d3f15090 100644 --- a/controllers/easynav_regulated_pp_controller/include/easynav_regulated_pp_controller/RegulatedPurePursuitController.hpp +++ b/controllers/easynav_regulated_pp_controller/include/easynav_regulated_pp_controller/RegulatedPurePursuitController.hpp @@ -166,7 +166,7 @@ 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; + 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( diff --git a/controllers/easynav_regulated_pp_controller/src/easynav_regulated_pp_controller/RegulatedPurePursuitController.cpp b/controllers/easynav_regulated_pp_controller/src/easynav_regulated_pp_controller/RegulatedPurePursuitController.cpp index 761dd28b..cfaddb9a 100644 --- a/controllers/easynav_regulated_pp_controller/src/easynav_regulated_pp_controller/RegulatedPurePursuitController.cpp +++ b/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 { diff --git a/controllers/easynav_regulated_pp_controller/tests/regulated_pp_controller_tests.cpp b/controllers/easynav_regulated_pp_controller/tests/regulated_pp_controller_tests.cpp index 2c9c595e..b8895e52 100644 --- a/controllers/easynav_regulated_pp_controller/tests/regulated_pp_controller_tests.cpp +++ b/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; From 08dd420b6274dc81c4365bdf909e54a28bfc352d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Thu, 13 Aug 2026 12:18:14 +0200 Subject: [PATCH 2/2] Add doxygen description MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- .../RegulatedPurePursuitController.hpp | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/controllers/easynav_regulated_pp_controller/include/easynav_regulated_pp_controller/RegulatedPurePursuitController.hpp b/controllers/easynav_regulated_pp_controller/include/easynav_regulated_pp_controller/RegulatedPurePursuitController.hpp index d3f15090..ed490523 100644 --- a/controllers/easynav_regulated_pp_controller/include/easynav_regulated_pp_controller/RegulatedPurePursuitController.hpp +++ b/controllers/easynav_regulated_pp_controller/include/easynav_regulated_pp_controller/RegulatedPurePursuitController.hpp @@ -166,6 +166,14 @@ class RegulatedPurePursuitController : public ControllerMethodBase double robot_yaw); /// \brief Whether the robot should rotate in place towards \p angle_to_path. + /// \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.