gridmap_2d: Adding conversion back to OccupancyGrid msg