Adaptive pure pursuit for path following

[ankithu]

Right now we have a pure pursuit-ish approach to following paths, we look on the current segment a certain distance ahead and then target that point, but if we are within a certain distance of the end point we just target the endpoint. "real" pure pursuit does not just target the end point of the segment if you are within the lookahead distance of it instead it looks ahead onto the next segment. This is not crazy to implement but this goes further when we consider adaptive pure pursuit which "adaptively" adjusts the lookahead distance to further optimize the path following, see the adaptive pure pursuit paper for more details.
https://github.com/umrover/mrover-ros/wiki/Adaptive-Pure-Pursuit

[qhdwight]

https://arxiv.org/abs/2111.08873