Cleaned up global_planner.cpp