Repository: ros-navigation/navigation2 (C++)

Issue:
stateful in regulated pure pursuit controller
## Feature request

#### Feature description
We are currently using the regulated pure pursuit controller along with a stateful simple goal checker. The issue arises when the robot reaches the goal within the specified XY tolerance: it begins to rotate toward the desired goal heading. However, during this rotation, the robot slightly drifts outside the XY tolerance, which causes it to interrupt the heading alignment and instead try to reposition itself within the XY tolerance again.

This creates a loop where the robot continuously switches between aligning its heading and repositioning in XY space, as shouldRotateToGoalHeading(carrot_pose) returns false when the XY condition is not met. Eventually, the robot may succeed by chance in meeting both conditions simultaneously, but this behavior is inefficient and unstable.

The root cause is that the controller itself lacks built-in statefulness—it doesn’t persist a state that prioritizes heading alignment once the XY goal has been reached.

#### Implementation considerations
My proposed solution is to add a `stateful` parameter to the Regulated Pure Pursuit (RPP) controller. When enabled, this parameter would allow the controller to persist the state once the robot has reached the goal within the XY tolerance. From that point, it would focus solely on aligning the heading.




Fix (patch):
diff --git a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/parameter_handler.hpp b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/parameter_handler.hpp
index 7b1be9855d2..21edd45558c 100644
--- a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/parameter_handler.hpp
+++ b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/parameter_handler.hpp
@@ -61,6 +61,7 @@ struct Parameters
   bool interpolate_curvature_after_goal;
   bool use_collision_detection;
   double transform_tolerance;
+  bool stateful;
 };
 
 /**
diff --git a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp
index df3e5ea587e..2d766ea967f 100644
--- a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp
+++ b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp
@@ -220,6 +220,7 @@ class RegulatedPurePursuitController : public nav2_core::Controller
   bool cancelling_ = false;
   bool finished_cancelling_ = false;
   bool is_rotating_to_heading_ = false;
+  bool has_reached_xy_tolerance_ = false;
 
   std::shared_ptr<rclcpp_lifecycle::LifecyclePublisher<nav_msgs::msg::Path>> global_path_pub_;
   std::shared_ptr<rclcpp_lifecycle::LifecyclePublisher<geometry_msgs::msg::PointStamped>>
diff --git a/nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp b/nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp
index e69a21875a4..27ed2a6628b 100644
--- a/nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp
+++ b/nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp
@@ -101,6 +101,8 @@ ParameterHandler::ParameterHandler(
   declare_parameter_if_not_declared(
     node, plugin_name_ + ".use_collision_detection",
     rclcpp::ParameterValue(true));
+  declare_parameter_if_not_declared(
+      node, plugin_name_ + ".stateful", rclcpp::ParameterValue(true));
 
   node->get_parameter(plugin_name_ + ".desired_linear_vel", params_.desired_linear_vel);
   params_.base_desired_linear_vel = params_.desired_linear_vel;
@@ -181,6 +183,7 @@ ParameterHandler::ParameterHandler(
   node->get_parameter(
     plugin_name_ + ".use_collision_detection",
     params_.use_collision_detection);
+  node->get_parameter(plugin_name_ + ".stateful", params_.stateful);
 
   if (params_.inflation_cost_scaling_factor <= 0.0) {
     RCLCPP_WARN(
@@ -308,6 +311,8 @@ ParameterHandler::updateParametersCallback(
         params_.use_cost_regulated_linear_velocity_scaling = parameter.as_bool();
       } else if (name == plugin_name_ + ".use_collision_detection") {
         params_.use_collision_detection = parameter.as_bool();
+      } else if (name == plugin_name_ + ".stateful") {
+        params_.stateful = parameter.as_bool();
       } else if (name == plugin_name_ + ".use_rotate_to_heading") {
         params_.use_rotate_to_heading = parameter.as_bool();
       } else if (name == plugin_name_ + ".use_cancel_deceleration") {
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 49617ea558e..2f43f427f2f 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
@@ -320,8 +320,21 @@ bool RegulatedPurePursuitController::shouldRotateToGoalHeading(
   const geometry_msgs::msg::PoseStamped & carrot_pose)
 {
   // Whether we should rotate robot to goal heading
-  double dist_to_goal = std::hypot(carrot_pose.pose.position.x, carrot_pose.pose.position.y);
-  return params_->use_rotate_to_heading && dist_to_goal < goal_dist_tol_;
+  if (!params_->use_rotate_to_heading) {
+    return false;
+  }
+
+  double dist_to_goal = std::hypot(
+    carrot_pose.pose.position.x, carrot_pose.pose.position.y);
+
+  if (params_->stateful) {
+    if (!has_reached_xy_tolerance_ && dist_to_goal < goal_dist_tol_) {
+      has_reached_xy_tolerance_ = true;
+    }
+    return has_reached_xy_tolerance_;
+  }
+
+  return dist_to_goal < goal_dist_tol_;
 }
 
 void RegulatedPurePursuitController::rotateToHeading(
@@ -466,6 +479,7 @@ void RegulatedPurePursuitController::applyConstraints(
 
 void RegulatedPurePursuitController::setPlan(const nav_msgs::msg::Path & path)
 {
+  has_reached_xy_tolerance_ = false;
   path_handler_->setPlan(path);
 }
 
@@ -493,6 +507,7 @@ void RegulatedPurePursuitController::reset()
 {
   cancelling_ = false;
   finished_cancelling_ = false;
+  has_reached_xy_tolerance_ = false;
 }
 
 double RegulatedPurePursuitController::findVelocitySignChange(