Waypoint handler publishes desired pose topic.

Vehicle controller publishes twists based on the desired poses.