Support rerouting when navigation to a waypoint fails

[maxbogue]

An idea that might assist with this: have obstacle avoidance only hold the waypoints for the current goal, to make it easier to handle clearing them and setting up new ones.