栅格地图
在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中的地图似乎采用的就不是三值化的地图，它存在更多的细节数据，则有可能会执行到后续语句。对于我们这里而言，目前只要了解前面这一部分差不多就可以了。
