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.