浙大这套开源的代码写得很nice 很值得借鉴 ， 对于 GridMap 类的实现。该类通过智能指针的封装简化了 GridMap 实例的创建和管理过程。一旦通过 GridMap::initMap(ros::NodeHandle &nh) 方法初始化，就可以方便地调用 GridMap 及其所有相关功能
它主要订阅了d435i的深度话题以及odom来实现的建图
typedef std::shared_ptr<GridMap> Ptr;
单独写一个node试一下
#include <ros/ros.h>
#include "plan_env/grid_map.h"GridMap::Ptr grid_map_test_ ; int main(int argc, char** argv)
{ros::init(argc, argv, "gridmap_test_node");ros::NodeHandle nh_("~");grid_map_test_.reset(new GridMap) ;grid_map_test_->initMap(nh_) ; ros::spin();return 0;
}
然后再在原来的基础上搞一个launch
<launch><node pkg="ego_planner" name="gridmap_test_node" type="gridmap_test_node" output="screen"><remap from="~grid_map/odom" to="/mavros/local_position/odom"/><remap from="~grid_map/cloud" to="nouse2"/><remap from="~grid_map/pose" to = "nouse1"/> <remap from="~grid_map/depth" to = "/iris/realsense/depth_camera/depth/image_raw"/><param name="grid_map/resolution" value="0.15" /> <param name="grid_map/map_size_x" value="50" /> <param name="grid_map/map_size_y" value="25" /> <param name="grid_map/map_size_z" value="3.0" /> <param name="grid_map/local_update_range_x" value="5.5" /> <param name="grid_map/local_update_range_y" value="5.5" /> <param name="grid_map/local_update_range_z" value="4.5" /> <param name="grid_map/obstacles_inflation" value="0.299" /> <param name="grid_map/local_map_margin" value="10"/><param name="grid_map/ground_height" value="-0.01"/><!-- camera parameter --><param name="grid_map/cx" value="376.0"/><param name="grid_map/cy" value="240.0"/><param name="grid_map/fx" value="319.9988245765257"/><param name="grid_map/fy" value="319.9988245765257"/><!-- depth filter --><param name="grid_map/use_depth_filter" value="true"/><param name="grid_map/depth_filter_tolerance" value="0.15"/><param name="grid_map/depth_filter_maxdist" value="5.0"/><param name="grid_map/depth_filter_mindist" value="0.2"/><param name="grid_map/depth_filter_margin" value="2"/><param name="grid_map/k_depth_scaling_factor" value="1000.0"/><param name="grid_map/skip_pixel" value="2"/><!-- local fusion --><param name="grid_map/p_hit" value="0.65"/><param name="grid_map/p_miss" value="0.35"/><param name="grid_map/p_min" value="0.12"/><param name="grid_map/p_max" value="0.90"/><param name="grid_map/p_occ" value="0.80"/><param name="grid_map/min_ray_length" value="0.3"/><param name="grid_map/max_ray_length" value="5.0"/><param name="grid_map/visualization_truncate_height" value="1.8"/><param name="grid_map/show_occ_time" value="false"/><param name="grid_map/pose_type" value="2"/> <param name="grid_map/frame_id" value="world"/></node><node pkg="odom_visualization" name="drone_odom_visualization" type="odom_visualization" output="screen"><remap from="~odom" to="/mavros/local_position/odom"/><param name="color/a" value="1.0"/> <param name="color/r" value="0.0"/> <param name="color/g" value="0.0"/> <param name="color/b" value="0.0"/> <param name="covariance_scale" value="100.0"/> <param name="robot_scale" value="1.0"/><param name="tf45" value="false"/><param name="drone_id" value="0"/></node><node name="rviz" pkg="rviz" type="rviz" args="-d $(find ego_planner)/launch/gridmap_test.rviz" required="true" /></launch>
最后用gazebo进行仿真
1. 在整个ego中调用的地方
// 位于/home/Fast-Drone-250/src/planner/plan_manage/src/planner_manager.cpp下的29行 grid_map_.reset(new GridMap);//GridMap::Ptr grid_map_; 在h文件声明 ， 这里使用了c++ 智能指针 ， 将grid_map_ 指向一个新的对象 原来的对象（如果有的话）将被销毁。grid_map_->initMap(nh);//传入nh 开始初始化Gridmapbspline_optimizer_->a_star_.reset(new AStar);bspline_optimizer_->a_star_->initGridMap(grid_map_, Eigen::Vector3i(100, 100, 100));//将该指针传入到a_star_->initGridMap2. 初始化栅格地图GridMap::initMap(ros::NodeHandle &nh)
初始化地图的相关参数(mp_) 以及数据结构(md_) ， 以及ros的回调函数以及消息发布器
void GridMap::initMap(ros::NodeHandle &nh)
{node_ = nh;/* get parameter */double x_size, y_size, z_size;node_.param("grid_map/resolution", mp_.resolution_, -1.0);node_.param("grid_map/map_size_x", x_size, -1.0);node_.param("grid_map/map_size_y", y_size, -1.0);node_.param("grid_map/map_size_z", z_size, -1.0);node_.param("grid_map/local_update_range_x", mp_.local_update_range_(0), -1.0);node_.param("grid_map/local_update_range_y", mp_.local_update_range_(1), -1.0);node_.param("grid_map/local_update_range_z", mp_.local_update_range_(2), -1.0);node_.param("grid_map/obstacles_inflation", mp_.obstacles_inflation_, -1.0);node_.param("grid_map/fx", mp_.fx_, -1.0);node_.param("grid_map/fy", mp_.fy_, -1.0);node_.param("grid_map/cx", mp_.cx_, -1.0);node_.param("grid_map/cy", mp_.cy_, -1.0);node_.param("grid_map/use_depth_filter", mp_.use_depth_filter_, true);node_.param("grid_map/depth_filter_tolerance", mp_.depth_filter_tolerance_, -1.0);node_.param("grid_map/depth_filter_maxdist", mp_.depth_filter_maxdist_, -1.0);node_.param("grid_map/depth_filter_mindist", mp_.depth_filter_mindist_, -1.0);node_.param("grid_map/depth_filter_margin", mp_.depth_filter_margin_, -1);node_.param("grid_map/k_depth_scaling_factor", mp_.k_depth_scaling_factor_, -1.0);node_.param("grid_map/skip_pixel", mp_.skip_pixel_, -1);node_.param("grid_map/p_hit", mp_.p_hit_, 0.70);node_.param("grid_map/p_miss", mp_.p_miss_, 0.35);node_.param("grid_map/p_min", mp_.p_min_, 0.12);node_.param("grid_map/p_max", mp_.p_max_, 0.97);node_.param("grid_map/p_occ", mp_.p_occ_, 0.80);node_.param("grid_map/min_ray_length", mp_.min_ray_length_, -0.1);node_.param("grid_map/max_ray_length", mp_.max_ray_length_, -0.1);node_.param("grid_map/visualization_truncate_height", mp_.visualization_truncate_height_, -0.1);node_.param("grid_map/virtual_ceil_yp", mp_.virtual_ceil_yp_, -0.1);node_.param("grid_map/virtual_ceil_yn", mp_.virtual_ceil_yn_, -0.1);node_.param("grid_map/show_occ_time", mp_.show_occ_time_, false);node_.param("grid_map/pose_type", mp_.pose_type_, 1);node_.param("grid_map/frame_id", mp_.frame_id_, string("world"));node_.param("grid_map/local_map_margin", mp_.local_map_margin_, 1);node_.param("grid_map/ground_height", mp_.ground_height_, 1.0);node_.param("grid_map/odom_depth_timeout", mp_.odom_depth_timeout_, 1.0);mp_.resolution_inv_ = 1 / mp_.resolution_;mp_.map_origin_ = Eigen::Vector3d(-x_size / 2.0, -y_size / 2.0, mp_.ground_height_);mp_.map_size_ = Eigen::Vector3d(x_size, y_size, z_size);//针对地图概率的参数，计算其对应的对数概率值mp_.prob_hit_log_ = logit(mp_.p_hit_);mp_.prob_miss_log_ = logit(mp_.p_miss_);mp_.clamp_min_log_ = logit(mp_.p_min_);mp_.clamp_max_log_ = logit(mp_.p_max_);mp_.min_occupancy_log_ = logit(mp_.p_occ_);mp_.unknown_flag_ = 0.01;cout << "hit: " << mp_.prob_hit_log_ << endl;cout << "miss: " << mp_.prob_miss_log_ << endl;cout << "min log: " << mp_.clamp_min_log_ << endl;cout << "max: " << mp_.clamp_max_log_ << endl;cout << "thresh log: " << mp_.min_occupancy_log_ << endl;for (int i = 0; i < 3; ++i)mp_.map_voxel_num_(i) = ceil(mp_.map_size_(i) / mp_.resolution_);mp_.map_min_boundary_ = mp_.map_origin_;mp_.map_max_boundary_ = mp_.map_origin_ + mp_.map_size_;//初始化数据 ， 分配内存int buffer_size = mp_.map_voxel_num_(0) * mp_.map_voxel_num_(1) * mp_.map_voxel_num_(2);md_.occupancy_buffer_ = vector<double>(buffer_size, mp_.clamp_min_log_ - mp_.unknown_flag_);md_.occupancy_buffer_inflate_ = vector<char>(buffer_size, 0);md_.count_hit_and_miss_ = vector<short>(buffer_size, 0);md_.count_hit_ = vector<short>(buffer_size, 0);md_.flag_rayend_ = vector<char>(buffer_size, -1);md_.flag_traverse_ = vector<char>(buffer_size, -1);md_.raycast_num_ = 0;md_.proj_points_.resize(640 * 480 / mp_.skip_pixel_ / mp_.skip_pixel_);md_.proj_points_cnt = 0;md_.cam2body_ << 0.0, 0.0, 1.0, 0.0,-1.0, 0.0, 0.0, 0.0,0.0, -1.0, 0.0, 0.0,0.0, 0.0, 0.0, 1.0;/* init callback */depth_sub_.reset(new message_filters::Subscriber<sensor_msgs::Image>(node_, "grid_map/depth", 50));extrinsic_sub_ = node_.subscribe<nav_msgs::Odometry>("/vins_fusion/extrinsic", 10, &GridMap::extrinsicCallback, this); //subif (mp_.pose_type_ == POSE_STAMPED){pose_sub_.reset(new message_filters::Subscriber<geometry_msgs::PoseStamped>(node_, "grid_map/pose", 25));sync_image_pose_.reset(new message_filters::Synchronizer<SyncPolicyImagePose>(SyncPolicyImagePose(100), *depth_sub_, *pose_sub_));sync_image_pose_->registerCallback(boost::bind(&GridMap::depthPoseCallback, this, _1, _2));}else if (mp_.pose_type_ == ODOMETRY){odom_sub_.reset(new message_filters::Subscriber<nav_msgs::Odometry>(node_, "grid_map/odom", 100, ros::TransportHints().tcpNoDelay()));sync_image_odom_.reset(new message_filters::Synchronizer<SyncPolicyImageOdom>(SyncPolicyImageOdom(100), *depth_sub_, *odom_sub_)); //消息同步器 /*它使用 message_filters::Synchronizer 类来将两个不同的消息流进行同步。这个同步器被配置为接受一个最大队列长度为 100 的消息缓冲区，并且使用 SyncPolicyImageOdom 作为同步策略。*/sync_image_odom_->registerCallback(boost::bind(&GridMap::depthOdomCallback, this, _1, _2));//用于同步接收深度图像和里程计数据，并在数据匹配时调用 GridMap 类中的 depthOdomCallback 函数进行处理}// use odometry and point cloudindep_cloud_sub_ =node_.subscribe<sensor_msgs::PointCloud2>("grid_map/cloud", 10, &GridMap::cloudCallback, this);indep_odom_sub_ =node_.subscribe<nav_msgs::Odometry>("grid_map/odom", 10, &GridMap::odomCallback, this);//定时器 更新地图占据信息occ_timer_ = node_.createTimer(ros::Duration(0.05), &GridMap::updateOccupancyCallback, this);
//可视化地图 vis_timer_ = node_.createTimer(ros::Duration(0.11), &GridMap::visCallback, this);//这里就是发布地图map_pub_ = node_.advertise<sensor_msgs::PointCloud2>("grid_map/occupancy", 10);map_inf_pub_ = node_.advertise<sensor_msgs::PointCloud2>("grid_map/occupancy_inflate", 10);md_.occ_need_update_ = false;md_.local_updated_ = false;md_.has_first_depth_ = false;md_.has_odom_ = false;md_.has_cloud_ = false;md_.image_cnt_ = 0;md_.last_occ_update_time_.fromSec(0);md_.fuse_time_ = 0.0;md_.update_num_ = 0;md_.max_fuse_time_ = 0.0;md_.flag_depth_odom_timeout_ = false;md_.flag_use_depth_fusion = false;// rand_noise_ = uniform_real_distribution<double>(-0.2, 0.2);// rand_noise2_ = normal_distribution<double>(0, 0.2);// random_device rd;// eng_ = default_random_engine(rd());
}
其中在EGO中 使用的是pose_type_ == ODOMETRY 所以在使用的时候只需要订阅odom 以及depth 话题
if (mp_.pose_type_ == POSE_STAMPED){pose_sub_.reset(new message_filters::Subscriber<geometry_msgs::PoseStamped>(node_, "grid_map/pose", 25));sync_image_pose_.reset(new message_filters::Synchronizer<SyncPolicyImagePose>(SyncPolicyImagePose(100), *depth_sub_, *pose_sub_));sync_image_pose_->registerCallback(boost::bind(&GridMap::depthPoseCallback, this, _1, _2));}else if (mp_.pose_type_ == ODOMETRY){odom_sub_.reset(new message_filters::Subscriber<nav_msgs::Odometry>(node_, "grid_map/odom", 100, ros::TransportHints().tcpNoDelay()));sync_image_odom_.reset(new message_filters::Synchronizer<SyncPolicyImageOdom>(SyncPolicyImageOdom(100), *depth_sub_, *odom_sub_));sync_image_odom_->registerCallback(boost::bind(&GridMap::depthOdomCallback, this, _1, _2));}
<param name="grid_map/pose_type" value="2"/> <arg name="odometry_topic" value="/vins_fusion/imu_propagate"/><arg name="camera_pose_topic" value="nouse1"/><arg name="depth_topic" value="/camera/depth/image_rect_raw"/><arg name="cloud_topic" value="nouse2"/><remap from="~grid_map/odom" to="$(arg odometry_topic)"/><remap from="~grid_map/cloud" to="$(arg cloud_topic)"/><remap from="~grid_map/pose" to = "$(arg camera_pose_topic)"/> <remap from="~grid_map/depth" to = "$(arg depth_topic)"/>
3.最主要的两个定时器
更新地图占据信息
occ_timer_ = node_.createTimer(ros::Duration(0.05), &GridMap::updateOccupancyCallback, this);
主要是更新地图的占据信息 包含的主要函数：
1. 更新深度信息：调用 projectDepthImage() 函数，将深度图像中的像素点投影到世界坐标系中，生成一系列3D点。
2. 射线投影：调用 raycastProcess() 函数，使用射线投影法将投影点与地图相交，更新地图的占据信息。
3. 如果本地地图已经更新，调用 clearAndInflateLocalMap() 函数，用于清除和膨胀本地地图。
（没怎么细看，以后有时间再回来看看）
可视化地图
vis_timer_ = node_.createTimer(ros::Duration(0.11), &GridMap::visCallback, this);
这个定时器用来发布地图信息和膨胀后的地图信息 其中两个重要的函数是：
1.publishMap()
2.publishMapInflate
3.1 projectDepthImage()
主要是将深度信息中的像素点坐标转换到世界坐标系下
void GridMap::projectDepthImage()
{// md_.proj_points_.clear();md_.proj_points_cnt = 0; //清空存储投影点数组uint16_t *row_ptr;// int cols = current_img_.cols, rows = current_img_.rows;int cols = md_.depth_image_.cols;int rows = md_.depth_image_.rows;int skip_pix = mp_.skip_pixel_;double depth;
//相机的旋转矩阵 将深度信息中的像素点从相机转到世界坐标系Eigen::Matrix3d camera_r = md_.camera_r_m_;if (!mp_.use_depth_filter_){ //遍历像素点for (int v = 0; v < rows; v+=skip_pix){row_ptr = md_.depth_image_.ptr<uint16_t>(v);for (int u = 0; u < cols; u+=skip_pix){Eigen::Vector3d proj_pt;depth = (*row_ptr++) / mp_.k_depth_scaling_factor_;proj_pt(0) = (u - mp_.cx_) * depth / mp_.fx_;proj_pt(1) = (v - mp_.cy_) * depth / mp_.fy_;proj_pt(2) = depth;proj_pt = camera_r * proj_pt + md_.camera_pos_;if (u == 320 && v == 240)std::cout << "depth: " << depth << std::endl;md_.proj_points_[md_.proj_points_cnt++] = proj_pt;}}}/* use depth filter */else{if (!md_.has_first_depth_)md_.has_first_depth_ = true;else{Eigen::Vector3d pt_cur, pt_world, pt_reproj;Eigen::Matrix3d last_camera_r_inv;last_camera_r_inv = md_.last_camera_r_m_.inverse();const double inv_factor = 1.0 / mp_.k_depth_scaling_factor_;for (int v = mp_.depth_filter_margin_; v < rows - mp_.depth_filter_margin_; v += mp_.skip_pixel_){row_ptr = md_.depth_image_.ptr<uint16_t>(v) + mp_.depth_filter_margin_;for (int u = mp_.depth_filter_margin_; u < cols - mp_.depth_filter_margin_;u += mp_.skip_pixel_){depth = (*row_ptr) * inv_factor;row_ptr = row_ptr + mp_.skip_pixel_;// filter depth// depth += rand_noise_(eng_);// if (depth > 0.01) depth += rand_noise2_(eng_);if (*row_ptr == 0){depth = mp_.max_ray_length_ + 0.1;}else if (depth < mp_.depth_filter_mindist_){continue;}else if (depth > mp_.depth_filter_maxdist_){depth = mp_.max_ray_length_ + 0.1;}// project to world framept_cur(0) = (u - mp_.cx_) * depth / mp_.fx_;pt_cur(1) = (v - mp_.cy_) * depth / mp_.fy_;pt_cur(2) = depth;pt_world = camera_r * pt_cur + md_.camera_pos_;// if (!isInMap(pt_world)) {// pt_world = closetPointInMap(pt_world, md_.camera_pos_);// }md_.proj_points_[md_.proj_points_cnt++] = pt_world;// check consistency with last image, disabled...if (false){pt_reproj = last_camera_r_inv * (pt_world - md_.last_camera_pos_);double uu = pt_reproj.x() * mp_.fx_ / pt_reproj.z() + mp_.cx_;double vv = pt_reproj.y() * mp_.fy_ / pt_reproj.z() + mp_.cy_;if (uu >= 0 && uu < cols && vv >= 0 && vv < rows){if (fabs(md_.last_depth_image_.at<uint16_t>((int)vv, (int)uu) * inv_factor -pt_reproj.z()) < mp_.depth_filter_tolerance_){md_.proj_points_[md_.proj_points_cnt++] = pt_world;}}else{md_.proj_points_[md_.proj_points_cnt++] = pt_world;}}}}}}/* maintain camera pose for consistency check */md_.last_camera_pos_ = md_.camera_pos_;md_.last_camera_r_m_ = md_.camera_r_m_;md_.last_depth_image_ = md_.depth_image_;
}
3.2 raycastProcess（）
使用射线投影法将投影点与地图相交，更新地图的占据信息。
void GridMap::raycastProcess()
{// if (md_.proj_points_.size() == 0)if (md_.proj_points_cnt == 0)return;ros::Time t1, t2;md_.raycast_num_ += 1;int vox_idx;double length;// bounding box of updated regiondouble min_x = mp_.map_max_boundary_(0);double min_y = mp_.map_max_boundary_(1);double min_z = mp_.map_max_boundary_(2);double max_x = mp_.map_min_boundary_(0);double max_y = mp_.map_min_boundary_(1);double max_z = mp_.map_min_boundary_(2);RayCaster raycaster;Eigen::Vector3d half = Eigen::Vector3d(0.5, 0.5, 0.5);Eigen::Vector3d ray_pt, pt_w;for (int i = 0; i < md_.proj_points_cnt; ++i){pt_w = md_.proj_points_[i];// set flag for projected pointif (!isInMap(pt_w)){pt_w = closetPointInMap(pt_w, md_.camera_pos_);length = (pt_w - md_.camera_pos_).norm();if (length > mp_.max_ray_length_){pt_w = (pt_w - md_.camera_pos_) / length * mp_.max_ray_length_ + md_.camera_pos_;}vox_idx = setCacheOccupancy(pt_w, 0);}else{length = (pt_w - md_.camera_pos_).norm();if (length > mp_.max_ray_length_){pt_w = (pt_w - md_.camera_pos_) / length * mp_.max_ray_length_ + md_.camera_pos_;vox_idx = setCacheOccupancy(pt_w, 0);}else{vox_idx = setCacheOccupancy(pt_w, 1);}}max_x = max(max_x, pt_w(0));max_y = max(max_y, pt_w(1));max_z = max(max_z, pt_w(2));min_x = min(min_x, pt_w(0));min_y = min(min_y, pt_w(1));min_z = min(min_z, pt_w(2));// raycasting between camera center and pointif (vox_idx != INVALID_IDX){if (md_.flag_rayend_[vox_idx] == md_.raycast_num_){continue;}else{md_.flag_rayend_[vox_idx] = md_.raycast_num_;}}raycaster.setInput(pt_w / mp_.resolution_, md_.camera_pos_ / mp_.resolution_);while (raycaster.step(ray_pt)){Eigen::Vector3d tmp = (ray_pt + half) * mp_.resolution_;length = (tmp - md_.camera_pos_).norm();// if (length < mp_.min_ray_length_) break;vox_idx = setCacheOccupancy(tmp, 0);if (vox_idx != INVALID_IDX){if (md_.flag_traverse_[vox_idx] == md_.raycast_num_){break;}else{md_.flag_traverse_[vox_idx] = md_.raycast_num_;}}}}min_x = min(min_x, md_.camera_pos_(0));min_y = min(min_y, md_.camera_pos_(1));min_z = min(min_z, md_.camera_pos_(2));max_x = max(max_x, md_.camera_pos_(0));max_y = max(max_y, md_.camera_pos_(1));max_z = max(max_z, md_.camera_pos_(2));max_z = max(max_z, mp_.ground_height_);posToIndex(Eigen::Vector3d(max_x, max_y, max_z), md_.local_bound_max_);posToIndex(Eigen::Vector3d(min_x, min_y, min_z), md_.local_bound_min_);boundIndex(md_.local_bound_min_);boundIndex(md_.local_bound_max_);md_.local_updated_ = true;// update occupancy cached in queueEigen::Vector3d local_range_min = md_.camera_pos_ - mp_.local_update_range_;Eigen::Vector3d local_range_max = md_.camera_pos_ + mp_.local_update_range_;Eigen::Vector3i min_id, max_id;posToIndex(local_range_min, min_id);posToIndex(local_range_max, max_id);boundIndex(min_id);boundIndex(max_id);// std::cout << "cache all: " << md_.cache_voxel_.size() << std::endl;while (!md_.cache_voxel_.empty()){Eigen::Vector3i idx = md_.cache_voxel_.front();int idx_ctns = toAddress(idx);md_.cache_voxel_.pop();double log_odds_update =md_.count_hit_[idx_ctns] >= md_.count_hit_and_miss_[idx_ctns] - md_.count_hit_[idx_ctns] ? mp_.prob_hit_log_ : mp_.prob_miss_log_;md_.count_hit_[idx_ctns] = md_.count_hit_and_miss_[idx_ctns] = 0;if (log_odds_update >= 0 && md_.occupancy_buffer_[idx_ctns] >= mp_.clamp_max_log_){continue;}else if (log_odds_update <= 0 && md_.occupancy_buffer_[idx_ctns] <= mp_.clamp_min_log_){md_.occupancy_buffer_[idx_ctns] = mp_.clamp_min_log_;continue;}bool in_local = idx(0) >= min_id(0) && idx(0) <= max_id(0) && idx(1) >= min_id(1) &&idx(1) <= max_id(1) && idx(2) >= min_id(2) && idx(2) <= max_id(2);if (!in_local){md_.occupancy_buffer_[idx_ctns] = mp_.clamp_min_log_;}md_.occupancy_buffer_[idx_ctns] =std::min(std::max(md_.occupancy_buffer_[idx_ctns] + log_odds_update, mp_.clamp_min_log_),mp_.clamp_max_log_);}
}
3.3 发布地图和膨胀后的地图
void GridMap::publishMap()
{if (map_pub_.getNumSubscribers() <= 0)// 如果没有订阅者 直接返回减少不要的计算return;pcl::PointXYZ pt;pcl::PointCloud<pcl::PointXYZ> cloud;
//地图局部边界信息Eigen::Vector3i min_cut = md_.local_bound_min_;Eigen::Vector3i max_cut = md_.local_bound_max_;
// 计算局部地图边界的缩小范围int lmm = mp_.local_map_margin_ / 2;min_cut -= Eigen::Vector3i(lmm, lmm, lmm);max_cut += Eigen::Vector3i(lmm, lmm, lmm);//限制边界boundIndex(min_cut);boundIndex(max_cut);
//遍历地图 将适合的点添加到点云中 for (int x = min_cut(0); x <= max_cut(0); ++x)for (int y = min_cut(1); y <= max_cut(1); ++y)for (int z = min_cut(2); z <= max_cut(2); ++z){if (md_.occupancy_buffer_[toAddress(x, y, z)] < mp_.min_occupancy_log_)continue; //占据的概率小于阈值 跳过//计算点的位置Eigen::Vector3d pos;indexToPos(Eigen::Vector3i(x, y, z), pos);if (pos(2) > mp_.visualization_truncate_height_)continue;pt.x = pos(0);pt.y = pos(1);pt.z = pos(2);//符合的点添加到点云中cloud.push_back(pt);}cloud.width = cloud.points.size();cloud.height = 1;cloud.is_dense = true;cloud.header.frame_id = mp_.frame_id_;sensor_msgs::PointCloud2 cloud_msg;//转换点云为ros消息格式pcl::toROSMsg(cloud, cloud_msg);map_pub_.publish(cloud_msg);
}void GridMap::publishMapInflate(bool all_info)
{if (map_inf_pub_.getNumSubscribers() <= 0)return;pcl::PointXYZ pt;pcl::PointCloud<pcl::PointXYZ> cloud;Eigen::Vector3i min_cut = md_.local_bound_min_;Eigen::Vector3i max_cut = md_.local_bound_max_;if (all_info){int lmm = mp_.local_map_margin_;min_cut -= Eigen::Vector3i(lmm, lmm, lmm);max_cut += Eigen::Vector3i(lmm, lmm, lmm);}boundIndex(min_cut);boundIndex(max_cut);for (int x = min_cut(0); x <= max_cut(0); ++x)for (int y = min_cut(1); y <= max_cut(1); ++y)for (int z = min_cut(2); z <= max_cut(2); ++z){if (md_.occupancy_buffer_inflate_[toAddress(x, y, z)] == 0)continue;Eigen::Vector3d pos;indexToPos(Eigen::Vector3i(x, y, z), pos);if (pos(2) > mp_.visualization_truncate_height_)continue;pt.x = pos(0);pt.y = pos(1);pt.z = pos(2);cloud.push_back(pt);}cloud.width = cloud.points.size();cloud.height = 1;cloud.is_dense = true;cloud.header.frame_id = mp_.frame_id_;sensor_msgs::PointCloud2 cloud_msg;pcl::toROSMsg(cloud, cloud_msg);map_inf_pub_.publish(cloud_msg);// ROS_INFO("pub map");
}
3.4 AStar::initGridMap(GridMap::Ptr occ_map, const Eigen::Vector3i pool_size)
void AStar::initGridMap(GridMap::Ptr occ_map, const Eigen::Vector3i pool_size)
{POOL_SIZE_ = pool_size;CENTER_IDX_ = pool_size / 2;GridNodeMap_ = new GridNodePtr **[POOL_SIZE_(0)];for (int i = 0; i < POOL_SIZE_(0); i++){GridNodeMap_[i] = new GridNodePtr *[POOL_SIZE_(1)];for (int j = 0; j < POOL_SIZE_(1); j++){GridNodeMap_[i][j] = new GridNodePtr[POOL_SIZE_(2)];for (int k = 0; k < POOL_SIZE_(2); k++){GridNodeMap_[i][j][k] = new GridNode;}}}grid_map_ = occ_map;
}
