add avoidObstacleMsg and edit controller.cpp