读取pcd文件并发布nav_msgs::OccupancyGrid消息

pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>());
    cloud->points.resize(prior.size());
    for (const auto& point : prior) {
      pcl::PointXYZ pcl_point;
      pcl_point.x = point(0);
      pcl_point.y = point(1);
      pcl_point.z = point(2);
      cloud->points.push_back(pcl_point);
    }
    pcl::PointXYZ min;
    pcl::PointXYZ max;
    //利用pcl读取点云最小值
    pcl::getMinMax3D(*cloud, min, max);

    float resolution = options_.map_resolution;
    //栅格长与宽
    int num_x_cell = cartographer::common::RoundToInt((max.x - min.x) / resolution);
    int num_y_cell = cartographer::common::RoundToInt((max.y - min.y) / resolution);
    occupancy_grid_ = cartographer::common::make_unique<nav_msgs::OccupancyGrid>();
    occupancy_grid_->header.stamp = ros::Time::now();
    occupancy_grid_->header.frame_id = options_.map_frame;
    occupancy_grid_->info.map_load_time = occupancy_grid_->header.stamp;
    occupancy_grid_->info.resolution = resolution;

    occupancy_grid_->info.width = num_x_cell;
    occupancy_grid_->info.height = num_y_cell;
    occupancy_grid_->info.origin.position.x = min.x;
    occupancy_grid_->info.origin.position.y = min.y;
    occupancy_grid_->info.origin.position.z = 0.;
    occupancy_grid_->info.origin.orientation.w = 1.;
    occupancy_grid_->info.origin.orientation.x = 0.;
    occupancy_grid_->info.origin.orientation.y = 0.;
    occupancy_grid_->info.origin.orientation.z = 0.;
    occupancy_grid_->data.resize(num_x_cell * num_y_cell, -1);
    for (auto& pt : prior) {
      //1. to corresponding cell
      int index_x = cartographer::common::RoundToInt((pt(0) - min.x) / resolution);
      if (index_x < 0 || index_x >= num_x_cell)
        continue;
      int index_y = cartographer::common::RoundToInt((pt(1) - min.y) / resolution);
      if (index_y < 0 || index_y >= num_y_cell)
        continue;
      //2. give a fixed probability 概率值设为80
      int value = 80
      
      occupancy_grid_->data[index_x + index_y * num_x_cell] = value;
    }