Should avoid backtracking when it goes past first point of the global planner path