Refactoring

Summary: Added the waypoint_selector with launch file and the waypoint_navigator.