Reversed the plan vector

Reversed the plan vector so the local planner finds out how to reach the goal