testbed obstacle avoidance seems working now