Created waypoint handler to autopilot robot.

Also, minor renaming