讀取pcd檔案併發布nav_msgs::OccupancyGrid訊息
阿新 • • 發佈:2021-12-27
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; }