Use laser and kinect to build obstacle occupancy grid of moveit