Plan and Teach a Route