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

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

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)</