Works in simulation. Needs local planner recovery movement.