Pure pursuit is a simple lateral control algorithm for following a set of waypoints.
This should be implemented with a sufficient number of tests