Add stateful to regulated pure pursuit controller (#5167)

* add_stateful_to_regulated_pure_pursuit_controller

Signed-off-by: Jad haj mustafa <jhajmustafa@lely.com>

* fix naming

Signed-off-by: Jad haj mustafa <jhajmustafa@lely.com>

* fix naming left over

Signed-off-by: Jad haj mustafa <jhajmustafa@lely.com>

* resolve comments

Signed-off-by: Jad haj mustafa <jhajmustafa@lely.com>

* typo

Signed-off-by: Jad haj mustafa <jhajmustafa@lely.com>

* add unit test for both cases stateful and not stateful

Signed-off-by: Jad haj mustafa <jhajmustafa@lely.com>

---------

Signed-off-by: Jad haj mustafa <jhajmustafa@lely.com>

Author: jadhm

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;
 };
 
 /**

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>>

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(
@@ -307,6 +310,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") {

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(

nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp

@@ -498,8 +498,14 @@ TEST(RegulatedPurePursuitTest, lookaheadAPI)
 
 TEST(RegulatedPurePursuitTest, rotateTests)
 {
+  // --------------------------
+  // Non-Stateful Configuration
+  // --------------------------
   auto ctrl = std::make_shared<BasicAPIRPP>();
   auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("testRPP");
+  nav2_util::declare_parameter_if_not_declared(
+    node, "PathFollower.stateful", rclcpp::ParameterValue(false));
+
   std::string name = "PathFollower";
   auto tf = std::make_shared<tf2_ros::Buffer>(node->get_clock());
   auto costmap = std::make_shared<nav2_costmap_2d::Costmap2DROS>("fake_costmap");
@@ -565,6 +571,27 @@ TEST(RegulatedPurePursuitTest, rotateTests)
   curr_speed.angular.z = 1.0;
   ctrl->rotateToHeadingWrapper(lin_v, ang_v, angle_to_path, curr_speed);
   EXPECT_NEAR(ang_v, 0.84, 0.01);
+
+  // -----------------------
+  // Stateful Configuration
+  // -----------------------
+  node->set_parameter(
+    rclcpp::Parameter("PathFollower.stateful", true));
+
+  ctrl->configure(node, name, tf, costmap);
+
+  // Start just outside tolerance
+  carrot.pose.position.x = 0.0;
+  carrot.pose.position.y = 0.26;
+  EXPECT_EQ(ctrl->shouldRotateToGoalHeadingWrapper(carrot), false);
+
+  // Enter tolerance (should set internal flag)
+  carrot.pose.position.y = 0.24;
+  EXPECT_EQ(ctrl->shouldRotateToGoalHeadingWrapper(carrot), true);
+
+  // Move outside tolerance again - still expect true (due to persistent state)
+  carrot.pose.position.y = 0.26;
+  EXPECT_EQ(ctrl->shouldRotateToGoalHeadingWrapper(carrot), true);
 }
 
 TEST(RegulatedPurePursuitTest, applyConstraints)
@@ -705,7 +732,8 @@ TEST(RegulatedPurePursuitTest, testDynamicParameter)
       rclcpp::Parameter("test.use_cost_regulated_linear_velocity_scaling", false),
       rclcpp::Parameter("test.inflation_cost_scaling_factor", 1.0),
       rclcpp::Parameter("test.allow_reversing", false),
-      rclcpp::Parameter("test.use_rotate_to_heading", false)});
+      rclcpp::Parameter("test.use_rotate_to_heading", false),
+      rclcpp::Parameter("test.stateful", false)});
 
   rclcpp::spin_until_future_complete(
     node->get_node_base_interface(),
@@ -736,6 +764,7 @@ TEST(RegulatedPurePursuitTest, testDynamicParameter)
       "test.use_cost_regulated_linear_velocity_scaling").as_bool(), false);
   EXPECT_EQ(node->get_parameter("test.allow_reversing").as_bool(), false);
   EXPECT_EQ(node->get_parameter("test.use_rotate_to_heading").as_bool(), false);
+  EXPECT_EQ(node->get_parameter("test.stateful").as_bool(), false);
 
   // Should fail
   auto results2 = rec_param->set_parameters_atomically(