got Turtlebot to go to the first and last waypoints