栅格地图

在ROS中，地图是非常基本的元素，特别对于2D激光SLAM而言，栅格地图可以说是必不可少的元素。机器人在需要前往目标点时，需要在栅格地图中找到一条合适的路径从当前点到达目标点，这部分内容在move_base中有了详细的接口，可以直接调用并返回路径。但是作为一名工程师，不仅要知其然更要知其所以然，正好最近重新看了下这部分的内容，在此简单对地图这块的处理简单作个笔记，以备后续翻阅。
熟悉move_base的开发者会知道，在ROS中，地图大致可以分为三张：栅格地图、障碍物地图与膨胀地图。当然也可以按照局部地图与全局地图划分，但是那个不是本篇的重点，暂时先不考虑那种划分方式。栅格地图与后续两张地图可以说是存在密不可分的关系。首先，栅格地图是作为障碍物地图的基础而存在的，没有栅格地图，亦没有障碍物地图。同时，栅格地图也是膨胀地图的必须元素，深入了解过三张地图之间相互关系的开发者会知道，在move_base中，膨胀地图这个插件（inflaction_layer）本身是不维护地图的实体的，它对于地图的膨胀是在栅格地图（静态地图）的图层进行操作的。所以有必要先了解清楚栅格地图的基础内容，这也可以为我们后续学习另外两种地图打下比较好的基础。

1、地图的消息格式

在ROS中已经定义了官方的地图格式：

nav_msgs/OccupancyGrid

我们可以通过下述指令展开看看这个消息的具体包含：

rosmsg show nav_msgs/OccupancyGrid

得到的结果如下

std_msgs/Header header
  uint32 seq
  time stamp
  string frame_id
nav_msgs/MapMetaData info
  time map_load_time
  float32 resolution
  uint32 width
  uint32 height
  geometry_msgs/Pose origin
    geometry_msgs/Point position
      float64 x
      float64 y
      float64 z
    geometry_msgs/Quaternion orientation
      float64 x
      float64 y
      float64 z
      float64 w
int8[] data

以上，就是一张地图消息格式的基本组成。我们从上往下看，首先是header部分，这里跟很多消息类型中的header是一样的，包含了一张地图的时间戳以及一个frame_id，对于地图这个东西而言，似乎时间戳并不是很重要。而frame_id则是一般会固定使用“map"。
其次，则是一个nav_msgs/MapMetaData实体info，其中的内容包含了较多信息。首先map_load_time，似乎也没什么用，所有地图设置的都是0。而resolution，则是地图中非常重要的一个参数，它直接关系到了地图的清晰度。一般而言，我们建图时使用的栅格大小就是这里的resolution，使用0.05代表一个栅格是5cm，而使用0.025则代表一个栅格是2.5cm。接下来的参数则是width与height，这两个参数与resolution共同构成了一张地图的实际长宽，例如：

resolution: 0.025
width: 2048
height: 2208

代表了地图的宽度为（0.025 * 2048=51.2）米，高度为（0.025 * 2208=55.2）米。最后面是一个geometry_msgs/Pose的实体，这个参数代表的则是这张地图上的一个坐标，开始时，我曾经一度以为这个坐标是指地图中心点的坐标轴，但是实际上并不是，它代表的是地图最左下角的点在地图上的坐标，这个问题我们可以通过一张实际的地图观察一下，首先我们拿到一张实际的地图的info信息：

map_load_time: 0.000000000
resolution: 0.025
width: 2048
height: 2208
origin: 
  position: 
    x: -20
    y: -35.2
    z: 0
  orientation: 
    x: 0
    y: 0
    z: 0
    w: 1

可以看到它的position为（-20，-35.2），然后我们在RVIZ中加载这张地图，并切换到地图坐标系下，可以看到地图大致显示如下：

这是一张51.2 * 55.2的地图，左上角处的栅格中心为（0，0）原点，大致就可以看出info参数中的position实际对应的就是地图的左下角部分。关于这个问题我们也可以后面通过一些简单的操作验证一下，先将数据结构看完。
剩下最后一个数据结构就是int8[] data这个字段，这个位置实际上存储的就是真正的地图实际数据，每一个栅格具体的值就是通过这里得到的，它的大小为info.width*info.height，同时，它的值只包含了三种数据：-1代表未知，也就是上图中灰色部分；0代表空闲，也就是图上的白色部分；100（注意是100不是1）代表障碍物，图上黑色部分。取一段实际地图数据，例如：

-1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 0 0 0 0 -1 -1 -1 -1 0 0 0 0 0 -1 -1 0 0 0 0 0 -1 0 0 0 0 0 0 0 0 -1 -1 0 0 0 0 -1 0 0 0 0 0 0 -1 -1 -1 0 0 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 100 100 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 100 100 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1

可以看到实际的数据就是如上所说的形式。

2、简单发布一张地图

为了验证上述一些问题，我们使用一个简单的节点对地图进行了简单的测试，代码过于简单，细节不赘述，先po源码上来：

#include "ros/ros.h"
#include "nav_msgs/OccupancyGrid.h"
#include <string>
#include <fstream>
#include <iostream>
#include <string>
#include <sstream>
class map_test2
{
private:
    /* data */
    ros::Publisher map_pub;
    nav_msgs::OccupancyGrid new_map;
public:
    map_test2(/* args */);
    void InitMap();
    void PubMap();
    ~map_test2();
};

map_test2::map_test2(/* args */)
{
    ros::NodeHandle n;
    map_pub = n.advertise<nav_msgs::OccupancyGrid>("map_test",2);
}

void map_test2::InitMap()
{
    new_map.header.seq = 1;
    new_map.header.stamp = ros::Time::now();
    new_map.header.frame_id = "map";
    new_map.info.height = 100;
    new_map.info.width = 100;
    new_map.info.map_load_time = ros::Time::now();
    new_map.info.resolution = 0.025;
    new_map.info.origin.position.x = 1.0;
    new_map.info.origin.position.y = 1.0;
    new_map.info.origin.orientation.w = 1.0;
    for(int i=0;i<new_map.info.height;i++)
    {
        for(int j=0;j<new_map.info.width;j++)
        {
            if((i%10)==0||(i%10)==1)
            {
                new_map.data.push_back(100);
            }
            else
                new_map.data.push_back(0);
        }
    }
}
void map_test2::PubMap()
{
    map_pub.publish(new_map);
}
map_test2::~map_test2()
{
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "map_test2");
    ROS_INFO("map_test2_node start");
    map_test2 map_test2;
    map_test2.InitMap();
    while (ros::ok())
    {
        map_test2.PubMap();
        ros::Duration(0.05).sleep();
        ros::spinOnce();
    }
    return 0;
}

上述代码实现了一个简单的地图发布功能，频率为20Hz，编译并运行上述代码，我们可以在RVIZ中看到一张实际名为map_test的地图：

注意到我们在上面给定地图原点的时候，给出的坐标系是

new_map.info.origin.position.x = 1.0;
new_map.info.origin.position.y = 1.0;

因此，其对应的栅格的左下角第一个点的位置就是在（1，1）这个位置。同时宽度与高度都是100，分辨率为0.025，因此，我们得到了一张宽与高都为2.5m的地图。最后，我们对于每10个栅格中行数末尾为0与1结尾的行设置了100（障碍物）的值，其他都设置了0（空闲），因此我们也得到了上述的带横向黑色细线条的具体地图实体。

由此，我们大致可以知道第一部分的分析基本上是没有问题的，对于地图部分的基础内容理解基本无误。

3、move_base中对于栅格地图的简单处理

为了后续其他研究需要，顺便简单记录一点move_base节点中对于栅格地图的处理部分。由于要考虑到在后续计算膨胀地图时使用的是0-255的表示方式，因此，在move_base中，对地图进行了简单的转换：

  // initialize the costmap with static data
  for (unsigned int i = 0; i < size_y; ++i)
  {
    for (unsigned int j = 0; j < size_x; ++j)
    {
      unsigned char value = new_map->data[index];
      costmap_[index] = interpretValue(value);
      ++index;
    }
  }

上述是一个遍历，根据地图的宽与高进行遍历，并调用interpretValue函数返回具体参数存储到costmap_中，而interpretValue实现如下：

unsigned char StaticLayer::interpretValue(unsigned char value)
{
  // check if the static value is above the unknown or lethal thresholds
  if (track_unknown_space_ && value == unknown_cost_value_)
    return NO_INFORMATION;
  else if (!track_unknown_space_ && value == unknown_cost_value_)
    return FREE_SPACE;
  else if (value >= lethal_threshold_)
    return LETHAL_OBSTACLE;
  else if (trinary_costmap_)
    return FREE_SPACE;
  double scale = (double) value / lethal_threshold_;
  return scale * LETHAL_OBSTACLE;
}

这个函数咋一看挺复杂，但是实际上很简单。首先我们先要明确其中的一些参数：track_unknown_space_这个参数字面意思跟踪未知空间，简单理解就是对未知空间的处理。我们可以看到第一个if与下面的else if是互斥的，也就是只能同时满足一个，而这个值默认为true，也就是走if语句。而unknown_cost_value_这个值为-1，对于-1，我们知道它在地图中代表的是什么？未知空间嘛。在这种情况下返回的数据是什么？NO_INFORMATION。这个值的实际值是255。如果这个时候你再看一下FREE_SPACE=0那么这个track_unknown_space_参数就很好理解了：就是对于原本地图中为未知的那部分区域，我是将它作为障碍物处理还是作为空闲处理嘛。然后下面第二个else if，将输入值跟lethal_threshold_比较，而这个值在算法中默认为100，大于等于100代表什么？障碍物。所以障碍物的点返回值为LETHAL_OBSTACLE（254）。接下来是第三个else if，这里没有作比较，只是判断了一个参数trinary_costmap_，如果trinary_costmap_为true，则直接返回FREE_SPACE（0）。而这个参数默认其实也是为true的，所以对于前面没有处理的数据在这里都会返回0。而实际上对于一张地图而言，我们判断完了它为未知情况的点（-1），判断完了它为障碍物的点（100），它本身也就只剩下为空闲（0）的点了。所以这里对其他点直接返回0本身是没有什么问题的。至于后面的：

double scale = (double) value / lethal_threshold_;
return scale * LETHAL_OBSTACLE;

想来应该是为了适配其他特殊情况而使用的，例如cartographer中的地图似乎采用的就不是三值化的地图，它存在更多的细节数据，则有可能会执行到后续语句。对于我们这里而言，目前只要了解前面这一部分差不多就可以了。

参考：
【ROS-Navigation】Costmap2D代价地图源码解读-静态层StaticLayer

栅格地图、障碍物地图与膨胀地图（栅格地图）_障碍物栅格膨胀-CSDN博客