Update obstacle_avoidance_pr2.cpp