Fix excessive route replans when the target is also an obstacle