栅格地图、障碍物地图与膨胀地图（障碍物地图（三）写一张障碍物地图）

花了不少时间看完了障碍物地图的大致思路，这里简单根据前面的思路来写一个简易版的障碍物地图。

1.订阅一张地图

首先，我们需要一张静态地图作为原始数据，这个我们可以订阅当前的map来获取：

void map_test1::MapCallback(const nav_msgs::OccupancyGrid::ConstPtr& map)
{
   
    map_origin = map->info.origin;
    ROS_INFO("map_origin.x:%f,y:%f",map_origin.position.x,map_origin.position.y);
    map_resolution = map->info.resolution;
    ROS_INFO("map_resolution:%f",map_resolution);
    map_width = map->info.width;
    map_height = map->info.height;
    ROS_INFO("map_width:%d",map_width);
    ROS_INFO("map_height:%d",map_height);
    raw_data.clear();
    for(int i=0;i<map->data.size();i++)
    {
   
        raw_data.push_back(map->data[i]);
    }
    first_receive = true;
    ROS_INFO("get raw map");
    map_sub.shutdown();
}

这里代码的处理比较简单，只是简单的存储了当前地图的基本信息数据，以给到后续代码使用。

2.订阅激光数据

第一部分我们获取到的只是最基本的原始地图数据，然后我们需要将障碍物添加到地图中去，也就是当前的激光点云，在原代码中，对于来自传感器的点云是存储了一定时间内的，这里我们按照原代码的思路也需要存储一定时间下的点云：

void map_test1::ScanCallback(const sensor_msgs::LaserScan::ConstPtr& scan_in)
{
      
    sensor_msgs::PointCloud mapcloud; 
    if(scan_in->header.frame_id.find("laser")==string::npos)
	    return;
	if(!tf_listener.waitForTransform(
            scan_in->header.frame_id,
            "/map",
            scan_in->header.stamp + ros::Duration().fromSec(scan_in->ranges.size()*scan_in->