easynav_regulated_pp_controller

Port of the Nav2 Regulated Pure Pursuit Controller to the EasyNav plugin architecture (easynav::ControllerMethodBase), including its optional Dynamic Window Pure Pursuit (DWPP) extension.

• S. Macenski, S. Singh, F. Martin, J. Gines, Regulated Pure Pursuit for Robot Path Tracking. Autonomous Robots, 2023.
• Fumiya Ohnishi and Masaki Takahashi, DWPP: Dynamic Window Pure Pursuit Considering Velocity and Acceleration Constraints, arXiv:2601.15006, 2026.

@article{macenski2023regulated,
      title={Regulated Pure Pursuit for Robot Path Tracking},
      author={Steve Macenski and Shrijit Singh and Francisco Martin and Jonatan Gines},
      year={2023},
      journal = {Autonomous Robots}
}

Algorithm

The controller finds a lookahead (“carrot”) point on the path at a given distance from the robot and drives towards it, following the classic Pure Pursuit geometry: the curvature needed to reach the carrot point is k = 2y / (x^2+y^2) in the robot’s local frame. On top of that, it adds the regulation terms from the paper above:

• Velocity-scaled lookahead distance (Adaptive Pure Pursuit): the lookahead distance grows with speed, clamped between min_lookahead_dist and max_lookahead_dist.
• Curvature regulation: linear velocity is reduced on sharp turns (small turning radius).
• Obstacle-proximity regulation: linear velocity is reduced close to obstacles.
• Approach-to-goal regulation: linear velocity is reduced smoothly as the robot nears the end of the path.
• Rotate to rough heading / rotate to goal heading: the robot rotates in place first if the carrot (or the final goal) is at a large angle from the current heading.
• Dynamic Window Pure Pursuit (optional): instead of directly applying angular_vel = linear_vel * curvature, it searches the reachable (velocity, angular velocity) space under the configured acceleration/deceleration limits for the point that best matches the desired curvature.

Adaptations from the Nav2 version

EasyNav’s design differs from Nav2’s controller_server in ways that require some adaptations:

• No costmap. EasyNav controllers only have access to the robot pose, the path, and fused point-cloud perceptions (PointPerception) via the NavState blackboard — there is no costmap or inflation layer to query. The cost-based linear velocity regulation term (use_cost_regulated_linear_velocity_scaling in Nav2) is therefore replaced by an equivalent term, use_obstacle_regulated_linear_velocity_scaling, driven by the distance to the nearest point in the fused perception cloud within the lookahead corridor, rather than by inflated costmap cost (see heuristics::obstacleConstraint in regulation_functions.hpp).
• No per-controller collision-arc checking. Nav2’s RPP throws a NoValidControl exception when it predicts a collision along a forward-simulated arc. EasyNav already performs this kind of safety stop uniformly for every controller in ControllerMethodBase (see the shared colision_checker.* parameters), so this controller does not duplicate it.
• No separate goal-checker plugin. Nav2 relies on an independent GoalChecker plugin to decide when the robot has reached the goal. This controller checks the distance/angle to the last path pose directly, using goal_tolerance.position / goal_tolerance.yaw from the NavState if GoalManager has published them, or the xy_goal_tolerance / yaw_goal_tolerance parameters otherwise — consistent with how other EasyNav controllers (e.g. easynav_simple_controller, easynav_mpc_controller) handle it.
• No TF-based path transform. EasyNav controllers assume path and robot_pose are already expressed in the same frame (see easynav_simple_controller, easynav_mpc_controller), so there is no transformPathInTargetFrame step. The path-to-robot-frame conversion needed by the Pure Pursuit geometry (curvature is only meaningful with the robot at the local origin facing +x) is done analytically from the robot pose instead of via tf2.
• No plan pruning upstream. Nav2’s controller_server prunes the global plan to a local window starting at (or near) the robot before handing it to the controller. EasyNav’s path in NavState is the full, un-pruned plan, so the carrot search (getLookAheadPoint) explicitly starts from the path pose closest to the robot (findClosestPoseIndex) rather than from index 0 of the array. Skipping this step is a real trap, not just a style choice: once the robot has travelled more than a lookahead distance away from the path’s start pose, that start pose itself would satisfy “farther than the lookahead distance”, so a naive search from index 0 keeps returning it as the carrot for the rest of the path — freezing the carrot behind the robot, producing bogus large curvature/angle values, and making the controller rotate-in-place far more than it should.
• No setSpeedLimit API. EasyNav has no equivalent to Nav2’s dynamic speed-limiting interface, so it is not ported.

Parameters

File truncated at 100 lines see the full file

Changelog for package easynav_regulated_pp_controller

0.4.0 (2026-07-26)

• Add missing easynav_sensors deps
• Fix overshoot when DWPP is activated
• Fix license and copyright
• Port of Regulated Pure Pursuit. Initial commit
• Contributors: Francisco Martín Rico

0.3.1 (2026-02-27 17:53)

0.2.1 (2026-02-27 14:48)

0.0.2 (2025-10-12)