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

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

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->time_increment),
           ros::Duration(1))){
        //ROS_INFO("timestamp error");
        return;
    }    
    try
    {
        projector_.transformLaserScanToPointCloud("/map",*scan_in,mapcloud,tf_listener);
    }
    catch(const std::exception& e)
    {
        ROS_ERROR("%s", e.what());
    }	
	mapcloud.header.frame_id = scan_in->header.frame_id;
	tf::StampedTransform transform;
    try{
      tf_listener.lookupTransform("/map", "/base_link",
                               ros::Time(0), transform);
    }
    catch (tf::TransformException &ex) {
        ROS_ERROR("%s",ex.what());
        ros::Duration(1.0).sleep();
        return;
    }
    Obstacle obstacle_cloud;
    obstacle_cloud.origin_.x = transform.getOrigin().getX();
    obstacle_cloud.origin_.y = transform.getOrigin().getY();
    obstacle_cloud.obstacle_range_ = 25.0;
    obstacle_cloud.cloud_.points = mapcloud.points;
	//processCloud(mapcloud);
    double now_time = ros::Time::now().toSec();
    laser_points.insert(std::make_pair(now_time, obstacle_cloud));
}

简单解析一下上述代码，首先第一部分是调用的ROS下的开源包transformLaserScanToPointCloud将激光点云在传感器坐标系转换到map坐标系下，然后同步存储该时刻下的机器人位姿（参考Observation类函数的形式），最后统一存储到obstacle_cloud下。注意这里我没有使用类函数的形式去保存，因为代码写的比较简单，直接使用了结构体进行的数据存储，然后再使用unordered_map的格式存储的一系列数据。通过这种方式实现了一个类似于Observation类以及ObservationBuffer的处理方式。

3.删除时间过久的数据

对于那些超过一定时间的点云，我们这里也是同样需要对其进行删除的，删除的方式也很简单，直接从unordered_map中删除掉这一条数据就可以了：

void map_test1::DeletePoint()
{
    
    if (laser_points.empty()) {
        return;
    }

    double now_time = ros::Time::now().toSec();
    auto ite = laser_points.begin();

    while (ite != laser_points.end()) {
        if (now_time - ite->first > dely_time) {
            // 删除当前元素，并预取下一个元素的迭代器
            laser_points.erase(ite++);
        } else {
            ++ite; // 正常移动到下一个元素
        }
    }
}

4.将激光点云添加到地图中

这里就用到了上一章中看过的东西了：

void map_test1::AddScanData()
{
    //拷贝原始数据
    obstacle_map_data = raw_data;
    //遍历激光点云
    unordered_map<double, Obstacle>::iterator ite;
    //ROS_INFO("MAP.SIZE:%zu",laser_points.size());
    for (ite = laser_points.begin(); ite != laser_points.end(); ite++) {
        //ROS_INFO("point.size:%ld",ite->second.cloud_.points.size());
        for(int i=0;i<ite->second.cloud_.points.size();i++)
        {
            //舍弃地图外的点
            if(!InMap(ite->second.cloud_.points[i].x,ite->second.cloud_.points[i].y))
            {
                continue;
            }
            //舍弃距离过远的点
            if(sqrt((ite->second.cloud_.points[i].x-ite->second.origin_.x)*(ite->second.cloud_.points[i].x-ite->second.origin_.x)+
            (ite->second.cloud_.points[i].y-ite->second.origin_.y)*(ite->second.cloud_.points[i].y-ite->second.origin_.y))>ite->second.obstacle_range_)
            {
                continue;
            }
            ModifyMap(ite->second.cloud_.points[i].x,ite->second.cloud_.points[i].y,obstacle_map_data);
        }
    }
}

首先，我们需要遍历之前存储的每一帧激光点云中的点，判断这个点是否在地图上：

bool map_test1::InMap(double x,double y)
{
    if(x>(map_origin.position.x+map_resolution*map_width) || x<map_origin.position.x)
    {
        //ROS_INFO("point not in map,point position:%f,%f",x,y);
        //ROS_INFO("map max_x:%f,min_x:%f",map_origin.position.x+map_resolution*map_width,map_origin.position.x);
        return false;
    }
    else if(y>(map_origin.position.y+map_resolution*map_height) || y<map_origin.position.y)
    {
        //ROS_INFO("point not in map,point position:%f,%f",x,y);
        //ROS_INFO("map max_y:%f,min_y:%f",map_origin.position.y+map_resolution*map_height,map_origin.position.y);
        return false;
    }
    return true;
}

对于那些不在地图中的数据我们就没有必要再进行后续的处理了。然后我们根据之前设置的obstacle_range_参数判断这个点到当时机器人本体所在位置的距离是否足够近，由此排除掉一些非常远的点。

//舍弃距离过远的点
            if(sqrt((ite->second.cloud_.points[i].x-ite->second.origin_.x)*(ite->second.cloud_.points[i].x-ite->second.origin_.x)+
            (ite->second.cloud_.points[i].y-ite->second.origin_.y)*(ite->second.cloud_.points[i].y-ite->second.origin_.y))>ite->second.obstacle_range_)
            {
                continue;
            }

然后，我们就可以对剩下的点调用ModifyMap函数修改对应点所在的地图栅格值：

void map_test1::ModifyMap(double x,double y,std::vector<int> &map)
{
    int mx,my;
    if(!worldToMap(x,y,mx,my))
    {
        return;
    }
    map[my * map_width + mx] = 100;
}

注意这里调用了worldToMap是根据源代码来写的，输入地图点的坐标返回的是栅格地图的XY索引。最后转化成一维坐标并修改对应的值：

bool map_test1::worldToMap(double wx, double wy, int& mx, int& my)
{
    if (wx < map_origin.position.x || wy < map_origin.position.y)
    return false;

    mx = (int)((wx - map_origin.position.x) / map_resolution);
    my = (int)((wy - map_origin.position.y) / map_resolution);

    if (mx < map_width && my < map_height)
        return true;

    return false;
}