Improve route replan detection

Fixes cases where bots would freeze
Improve local obstacle avoidance to handle map geometry