Feature request

Feature description

Currently, RPP decides when to trigger approach velocity scaling based on if the actual lookahead distance is different from the requested lookahead distance by a certain margin. This is problematic because lookahead distance is determined from current velocity, so there is a cyclic dependency that can lead to oscillations at the stop point.

graph TD
    velocity --> lookahead_distance --> velocity_scaling --> cmd_vel --> velocity

Implementation considerations

Rather than looking to lookahead distance as a proxy measure, I wonder if there is some way to pipe the distance to the goal from a goal checker? Because simple euclidean distance to the goal pose may not be compatible with all goal checkers.

Velocity smoothing and motion profiling has been suggested as a fix to this issue, but I think that we should be able to come up with a simple solution for RPP that fixes this underlying cyclic dependency in the algorithm.