Add validation to Location, RobotStatus, and Waypoint topics
Remove some duplicate code in Waypoint validation