Working version of global planner