add obstacle avoidance

Add obstacle avoidance.