Makes path planning use a queue of waypoints so it can progress