removed pushing the goal to the end of the path - this is now being done in the global planner