Waypoint functionality

Waypoints can be displayed on screen and can be visited by the robot
icon.