Improve goal avoidance and wall follow

Improve goal avoidance and wall follow.