膨胀地图（二）

前面看完了膨胀地图相关的内容，这里根据前面看过的内容手搓一张膨胀地图试一下。

1、数据预处理

第一步,先进行数据预处理，为了后续计算方便，首先在这里预先计算两张二维数组表，后续遍历时会用到这张表：

void map_test3::computeCaches()
{//Case 1:如果栅格的膨胀半径是0，那就直接返回。
    
    if (robot_radius == 0)
        return;
    cached_distances_.clear(); // 分配内存
    for (unsigned int i = 0; i <= 50; ++i) {
        for (unsigned int j = 0; j <= 50; ++j) {
            cached_distances_[i][j] = hypot(static_cast<double>(i), static_cast<double>(j));
            ROS_INFO("i:%d,j:%d,value:%f",i,j,cached_distances_[i][j]);
        }
    }

    ROS_INFO("computeCost");
    cached_costs_.clear();
    // 计算代价
    for (unsigned int i = 0; i <= 50; ++i) {
        for (unsigned int j = 0; j <= 50; ++j) {
            cached_costs_[i][j] = computeCost(hypot(static_cast<double>(i), static_cast<double>(j)));
            //ROS_INFO("i:%d,j:%d,value:%f",i,j,cached_costs_[i][j]);
        }
    }
}

这个函数内涉及到了两张表，第一张存储的是三角形的斜边长，它的意义是当前栅格在离它最近的障碍物点之间的栅格距离。而第二章表存储的是代价值，它与第一张表是相对应的，代表的是这个距离的cost值的变化。这里面还用到了一个computeCost函数，它与源码中的基本上是差不多的，只是源码中的变量weight这里暂时使用了固定值代替，但是不影响大体运行思路。

2、地图订阅与膨胀

在两张表处理完成后，接下来需要一张现有的地图，所以这里要进行一张地图的订阅与数据处理。地图的订阅比较简单，就是将数据存储到raw_data内，但是需要注意一个问题，那就是我们是不能直接对这张地图进行膨胀的，因为如果障碍物紧贴地图边缘的话，根据机器人大小向外膨胀时会超出地图边界，这样的话数据计算上会变得比较麻烦，因此，这里获取到原始数据后首先对其按照机器人大小进行膨胀，这样子后续就不用担心处理边界的问题：

void map_test3::updateBounds()
{
    int length = int(robot_radius/map_resolution)+1;
    ROS_INFO("length:%d",length);
    for(int i=0;i<map_height+2*length;i++)
    {
        for(int j=0;j<map_width+2*length;j++)
        {
            if(i<length)  //地图上边界膨胀
            {
                expend_data.push_back(-1);
            }
            else if(length <= i && i<map_height+length)
            {
                if(j<length)
                {
                    expend_data.push_back(-1);
                }
                else if(length<=j && j<map_width+length)
                {
                    expend_data.push_back(raw_data[map_width*(i-length)+j-length]);
                }
                else
                {
                    expend_data.push_back(-1);
                }
            }
            else
            {
                expend_data.push_back(-1);
            }
        }
    }
}

updateBounds函数的中心思想还是很简单的，它只需要将原始数据按顺序保留，在地图外部扩张出一部分栅格即可，对于这些扩张出来的栅格，先可以将其设置为未知（-1）。

3、障碍物点云的提取与膨胀

这一部分的思想与上一篇中源码的思想基本上是一致的，首先提取出地图中所有的障碍物点，然后对其进行遍历，更改其占用值，当然，对于这些点而言，它的占用值是不变的，然后，下一步更新其周围四个点的数据：

void map_test3::updateCosts()
{
    int length = int(robot_radius/map_resolution)+1;
    int seen_size_ = (map_height+2*length) * (map_width+2*length);
    if (seen_ == NULL) {// seen_的阵列为空，重新赋值
        ROS_WARN("InflationLayer::updateCosts(): seen_ array is NULL");
        int seen_size_ = (map_height+2*length) * (map_width+2*length);
        seen_ = new bool[seen_size_]();
    }
    else if (seen_size_ != (map_height+2*length) * (map_width+2*length))
    {
        ROS_WARN("InflationLayer::updateCosts(): seen_ array size is wrong");// seen_的阵列为错误，删除后重新赋值
        delete[] seen_;
        seen_size_ = (map_height+2*length) * (map_width+2*length);
        seen_ = new bool[seen_size_]();//这里分配了一个size 和master map 尺寸一样的bool 数组
    }
    else
    {
        if (seen_)
            delete[] seen_;
        seen_size_ = (map_height+2*length) * (map_width+2*length);
        seen_ = new bool[seen_size_]();
    }
    inflation_cells_.clear();
    inflation_cells.clear();
    map_seen.clear();
    for (int i = 0; i < map_height+2*length; i++)
    {
        for (int j = 0; j < map_width+2*length; j++)
        {
            int index = i*(map_width+2*length)+j;    //1)从master_grid地图获取(i,j)对应的索引index
            unsigned char cost = expend_data[index];  //2)从master_grid地图获取索引对应的代价cost
            if (cost == 100)               //3)先判断代价是否等于致命障碍物对应的代价，如果是，压入
            { //注意这里的obs_bin是一个指针，数据其实是存储在inflation_cells_下
                //std::map<double, std::vector<CellData> > inflation_cells_;
                CellData celldata;
                celldata.index_ = index;
                celldata.src_x_ = i;
                celldata.src_y_ = j;
                celldata.x_ = i;
                celldata.y_ = j;
                inflation_cells_[0].push_back(celldata);
                inflation_cells.push_back(celldata);
            }
        }
    }
    //根据膨胀队列，进行膨胀
    //通过上面的操作，现在inflation_queue_里面全部都是obstacle cell，这些cell被打包成CellData类型，包含了这些cell本身的索引，最近障碍物的索引（即它们本身），距离（0）
    std::map<double, std::vector<CellData> >::iterator bin;
    int count = 0;
    int interate = 0;
    while(inflation_cells.size()>0)// && count<12000)
    {
        CellData cell;
        cell = inflation_cells[0];
        //记录该celldata的索引index
        unsigned int index = cell.index_;

        // ignore if already visited  如果已访问过，则忽略
        if (seen_[index])
        {
            //ROS_INFO("index: %d has been seen",index);
            inflation_cells.erase(inflation_cells.begin());
            continue;
        }
        seen_[index] = true;
        map_seen[index] = true;
        //得到该cell的坐标和离它最近的障碍物的坐标
        unsigned int mx = cell.x_;
        unsigned int my = cell.y_;
        unsigned int sx = cell.src_x_;
        unsigned int sy = cell.src_y_;
        //ROS_INFO("mx:%d,my:%d,src_x:%d,src_y:%d",mx,my,sx,sy);
        //4)更新膨胀队列中点的cost 值
        //根据该CELL与障碍的距离来分配cost
        
        unsigned char cost = costLookup(mx, my, sx, sy);
        unsigned char old_cost = expend_data[index];  //从master_grid查找指定索引index的代价
        //如果old_cost无信息或者cost≥内接膨胀障碍物，将cost更新到master_array
        if (old_cost == NO_INFORMATION && (inflate_unknown_ ? (cost > FREE_SPACE) : (cost >= INSCRIBED_INFLATED_OBSTACLE)))
            expend_data[index] = cost;
        else//否则比较新旧大小，将大的cost更新到master_array
            expend_data[index] = std::max(old_cost, cost);
        ROS_INFO("index:%d",index);
        // attempt to put the neighbors of the current cell onto the inflation list
        //上面首先从inflation_cells_中取出最大distance的cell，再将这个cell的前后左右四个cell都塞进inflation_cells_。
        //由于下面关于enqueue的调用，最后两个参数（sx, sy)没有改变，所以保证了这个索引一定是obstacle cell。
        //由于在 enqueue入口会检查 if (!seen_[index])，这保证了这些cell不会被重复的塞进去。
        //由于这是一个priority queue，因此障碍物的周边点，每次都是离障碍物最远的点被弹出，并被检查，这样保证了这种扩张是朝着离障碍物远的方向进行。
        //尝试将当前单元的邻居(前后左右)放到队列中 
        //ROS_INFO("COUNT:%d",count);
        //if (mx > 0 && count<1000)
        if (mx > 0)
            enqueue(index - 1, mx - 1, my, sx, sy);
        if (my > 0)
            enqueue(index - (map_width+2*length), mx, my - 1, sx, sy);       
        if (mx < (map_width+2*length) - 1)
            enqueue(index + 1, mx + 1, my, sx, sy);
        if (my < (map_height+2*length) - 1)
            enqueue(index + (map_width+2*length), mx, my + 1, sx, sy);
        inflation_cells.erase(inflation_cells.begin());    
    }
}

这个函数的中心在于while循环以及循环内的enqueue函数，enqueue函数是非常重要的一个函数，它的主要内容如下：

inline void map_test3::enqueue(int index, unsigned int mx, unsigned int my,
                                    unsigned int src_x, unsigned int src_y)
{
  if (!seen_[index])
  {
    // we compute our distance table one cell further than the inflation radius dictates so we can make the check below
    //找它和最近的障碍物的距离，如果超过了阈值，则直接返回
    //如果不超过阈值，就把它也加入inflation_cells_数组里
    double distance = distanceLookup(mx, my, src_x, src_y);
    // we only want to put the cell in the list if it is within the inflation radius of the obstacle point
    if (distance*map_resolution > robot_radius)
      return;

    // push the cell data onto the inflation list and mark
    CellData celldata;
    celldata.index_ = index;
    celldata.x_ = mx;
    celldata.y_ = my;
    celldata.src_x_ = src_x;
    celldata.src_y_ = src_y;
    inflation_cells_[distance].push_back(celldata);
    inflation_cells.push_back(celldata);
  }
}

在enqueue函数中会判断当前点到最近障碍物的距离。如果处于一个危险范围内(distance*map_resolution < robot_radius)则会将该点加入到遍历的队列，后续会根据点到障碍物点的距离更新其代价值，如果这个点离障碍物足够远则可以认为当前这个点是足够安全的，则不需要更新该点，通过这种方式，可以将所有离障碍物点足够近的点都遍历一遍，并修改这些点的占用值为合适的数据。