栅格地图是将环境栅格化， 然后根据测量值对每个栅格的占有率进行计算， 通过概率计算判断给各栅格可能被障碍物占的几率， 并赋值一个0到1之间的值。通过激光雷达获取的大量数据帮助对每个栅格点进行准确判断。栅格地图中栅格大小决定了地图的精度， 也将影响机器人的定位精度，因此追求更小更多的栅格，但是栅格变小后，其数据量将会显著增加， 因此栅格地图不适合用于大型地图的构建， 适用于室内定位这种小型环境地图构建

地图在ROS中是以Topic的形式维护和呈现的，这个Topic名称就叫做/map，它的消息类型是nav_msgs/OccupancyGrid。由于/map中实际上存储的是一张图片，为了减少不必要的开销，这个Topic往往采用锁存（latched）的方式来发布。

锁存器的作用就是，将发布者最后一次发布的消息保存下来，然后把它自动发送给后来的订阅者。这种方式非常适合变动较慢、相对固定的数据（例如地图），然后只发布一次，相比于同样的消息不定的发布，锁存的方式既可以减少通信中对带宽的占用，也可以减少消息资源维护的开销。

nav_msgs/OccupancyGrid

map话题的消息类型是OccupancyGrid，通过rosmsg show nav_msgs/OccupancyGrid来查看

std_msgs/Header header       #消息的报头
    uint32 seq
    time stamp
    string frame_id         #地图消息绑定在TF的哪个frame上，一般为map

nav_msgs/MapMetaData info   #地图相关信息
    time map_load_time      #加载时间
    float32 resolution     #分辨率 单位：m/pixel
    uint32 width           #宽 单位：pixel
    uint32 height          #高 单位：pixel
    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       #地图具体信息

info是地图的配置信息，它反映了地图的属性。设置地图原点

// 将栅格地图原点设置在整张图的(-20, -20)位置
grid_map.info.origin.x = -20;
grid_map.info.origin.y = -20;

整张地图的障碍物信息存放在data数据成员中，data是一个int8类型的vector，它存储的内容有width * height个int8型的数据，也就是这张地图上每个像素。map中data存储的格式如下

0      空白区域
100    障碍物
-1     未知
1-99   根据像素的灰度值转换后的值，代表有障碍物的概率、

左下角像素点在世界坐标系下的位置为(origin_x，origin_y), 单位米。那么世界坐标系下的点(x,y)，转换到地图坐标系，那么该点对应的data中的索引index为：

index = (int)( (x - origin_x) / resolution) + ((int)((y - origin_y) / resolution)) * width;

该点在地图中的信息即为data[index]

一个宽和高的栅格地图，索引保存方式是这样的:

4        8        12     16

3        7        11     15

2        6        10     14

1        5        9      13

1相当于地图的原点，6就对应坐标(1, 1)。那么点坐标(2, 3)，对应的栅格索引是x * map.info.width + y = 12

pgm和yaml

运行rosrun map_server map_saver -f fileName可以保存地图，保存成功后，会有文件名对应的pgm和yaml文件。pgm文件就是地图的文件，标准的地图是用inkscape打开时看到的模样。

地图就是一张普通的灰度图像，通常为pgm格式。在地图中，黑色的网格是障碍区域，白色的网格是无障碍区域，灰色的网格是未知区域。map_server从图片中读取信息然后将每个网格都赋值[-1, 100]，灰色是-1，代表未知区域；白色是0，空闲区域；黑色是100，障碍区域。

yaml描述地图元数据，内容如下：

image: Software_Museum.pgm      #指定地图文件，可以是相对路径
resolution: 0.020000            #地图的分辨率 单位为 m/pixel
# 地图的原点，即地图左下像素的2D姿态（x，y，yaw），偏航为逆时针旋转，忽略偏航时为0
# 这个坐标体系用于让ros在图片里找到原点而已
origin: [-15.720000, -12.520000, 0.000000]
negate: 0                     # 0代表白色空闲，黑色占据。一般不用修改
occupied_thresh: 0.65         # 当占据的概率大于0.65认为被占据
free_thresh: 0.196            # 当占据的概率小于0.196认为无障碍
# 介于 occupied_thresh 和 free_thresh 的栅格unknown

其中占据的概率occ = (255-color_avg) / 255.0， color_avg为RGB三个通道的平均值。

• occupied_thresh：大于这个阀值的占用率的像素被认为occupied
• free_thresh：小于这个阀值的占用率的像素被认为是free

如果occupied_thresh设置的比较大，有些障碍物会没有障碍层和膨胀层。

如果建图时，障碍的边缘不够明显，也可能出现此问题，此时可以降低occupied_thresh。 其实还可以降低参数cost_scaling_factor，不过对膨胀层的影响较大。

• 栅格地图的分辨率不必太好，常用0.05

栅格地图的坐标系原点，是在栅格地图左下角，这里的origin是左下角在map坐标系的坐标，不是(0,0,0)

实际举例，先获得机器人的当前姿态：

然后在rviz上测量出机器人与左下角两个方向的距离

用机器人的坐标减去origin的坐标:

• x = -2.394 - (-15.72) = 13.326
• y = 2.37 - (-12.52) = 14.89

显然和测量结果大致相同，计算都是在map坐标系下。

map_server和map_saver

map_server是一个和地图相关的功能包，它可以将已知地图发布出来，供导航和其他功能使用，也可以保存SLAM建立的地图。它还提供了map_saver命令行实用程序，它允许动态生成的映射保存到文件。

要让map_server发布/map，需要yaml和pgm两个文件，然后可以通过指令来加载这张地图,map_server相关命令如下:

rosrun map_server map_server    test.yaml 	加载自定义的地图，通常使用roslaunch
rosrun map_server map_saver -f  mymap 	    保存当前地图为mymap.pgm和mymap.yaml

或者使用roslaunch加载地图：

<arg name="map_file" default="/home/hlhp/robot_ws/workspace/maps/test.yaml"/>
<node name="map_server" type="map_server" args="$(arg map_file)" pkg="map_server"/>

map_server节点提供的服务：

• static_map (nav_msgs/GetMap):提供检索地图服务

map_server节点读取pgm文件，通过服务static_map将地图数据提供给其他节点。比如move_base节点从中获取地图数据，用来进行路径规划；amcl节点用地图来进行定位，代码在AmclNode::requestMap()

map_server发布的话题包括：

/map_metadata    (nav_msgs/MapMetaData): 发布地图的描述信息，通过此锁存话题接收地图元数据。
/map     (nav_msgs/OccupancyGrid): 通过此锁存话题接收地图

加载yaml文件后,出现下面的结果:

运行rostopic echo map_metadata的结果:

map_load_time: 
  secs: 1569549222
  nsecs: 440982758
resolution: 0.019999999553
width: 1504
height: 2112
origin: 
  position: 
    x: -15.72
    y: -12.52
    z: 0.0
  orientation: 
    x: 0.0
    y: 0.0
    z: 0.0
    w: 1.0

echo一下map话题，结果是一大堆-1和0，这就是上面所说的occupancy grid，-1代表未知，0代表无障碍，100代表障碍。

参数：

• ~frame_id (string, default: “map”):地图坐标系的名称，要在已发布地图的标题中设置的框架， 绑定发布的地图与tf中的哪个frame，通常就是map

问题 1

有时运行rosrun map_server map_saver，结果显示 Wating for the map， 无法保存成功，进程阻塞了

问题 2

有几次，map_server打开pgm时一直阻塞，只能重启ROS

map_server::loadMapFromFile(&map_resp_,mapfname.c_str(),res,negate,occ_th,free_th, origin, mode);
ROS_INFO("map_server after loadMapFromFile");

// To make sure get a consistent time in simulation
ros::Time::waitForValid();

经过加日志，发现是 ros::Time::waitForValid();没有执行完，问题出在这里。

bool Time::waitForValid(const ros::WallDuration& timeout)
{
    ros::WallTime start = ros::WallTime::now();
    while (!isValid() && !g_stopped)
    {
        ros::WallDuration(0.01).sleep();

        if (timeout > ros::WallDuration(0, 0) && (ros::WallTime::now() - start > timeout))
        {
         return false;
        }
    }
    if (g_stopped)
    {
        return false;
    }
    return true;
}