should be implemented in pathfollower? little more challenging but not as esoteric as some of the other stuff