Can now update the costmap with kinect point cloud data