-Refactored waypoint_mgr
-Started work on refactoring the high level navigation code into navigator