add obstacle and lidar to sim env
working on obstacle avoidance