基于ROS实现在建图的同时显示本地地图背景的方法

基于ROS实现在建图的同时显示本地地图背景的方法

本文介绍如何通过ros的map_server工具包将创建的地图（ nav_msgs/OccupancyGrid格式）保存为本地文件，并通过读取本地文件重新回放在rviz界面上的方法。

1.安装map_server

对于Ubuntu16.04系统，使用如下命令安装：

sudo apt-get install ros-kinetic-map-server

对于Ubuntu18.04系统，使用如下命令安装：

sudo apt-get install ros-melodic-map-server

2.map_server功能包简介

• map_server包提供了一个map_server ROS 节点, 该节点通过ROS 服务器方式提供地图数据。
• 该包还提供了map_saver命令行工具（ROS节点）, 使用该工具可将动态创建的地图保存成文件。

关于map_server功能包的详细说明可参考ROS官方文档和相关的中文教程，本文不再赘述，下面重点介绍保存和回放地图的具体方法。

3.利用map_server将地图保存在本地

在得到动态创建的地图之后，可以利用map_server功能包中的map_saver节点将当前地图保存到指定文件名的本地文件中，map_saver节点的命令语法如下：

rosrun map_server map_saver [-f mapname]
rosrun map_server map_saver [--occ <threshold_occupied>] [--free <threshold_free>] [-f <mapname>]

其中，map_saver获取地图数据，并把它写到mapname.pgm和mapname.yaml本地文件中。

• -occ选项（可选）可以指定地图中被判定为障碍物的阈值（0-100）。
• -free选项（可选）可以指定地图中被判定为FreeSpace的阈值（0-100），且threshold_occupied的数值应大于threshold_free的值。
• -f选项（可选）可以指定地图的存放目录和名称。
  用法示例：

rosrun map_server map_saver --occ 40 -f lidar_map

即可设置地图的障碍物判定阈值为40，并在当前目录下保存名为lidar_map.pgm和lidar_map.yaml的地图文件。利用系统中的图像查看打开lidar_map.pgm文件，即可直接查看保存的地图结果。

4.利用map_server回放本地地图文件

map_server节点可以从磁盘读取地图文件并使用ROS service提供地图。
目前版本的map_server可将地图中的颜色值转化成三种占用值： 自由 (0), 占用 (100)和未知 (-1)。
map_server节点的命令语法如下：

rosrun map_server map_server mapname.yaml [_frame_id:=frame_id]

其中，mapname.yaml为指定的本地地图文件，_frame_id参数（可选）用于设置发布的地图的坐标系。
用法示例：

rosrun map_server map_server lidar_map.yaml _frame_id:=map_lidar

即可读取本地文件lidar_map.yaml获取地图信息，并发布新的/map话题，可用于地图的显示，同时_frame_id参数指定/map话题中消息的坐标系名称（frame_id）为map_lidar.

5.在rviz上显示回放的地图数据

利用map_server节点回放地图数据后，执行以下两个步骤，即可在rviz显示出保存在本地的地图数据：

• 在rviz的“Displays”窗口点击“ADD”按钮，选择“Map”类型的话题“/map”；
• 将rviz的“Displays”窗口中的选项“Global Options->Frixed Frame”设置为执行map_server节点时指定的“map_laser”。

6.在Cartographer建图的同时显示本地地图背景

由于Cartographer建图时发布的topic名称、map_saver节点订阅的topic名称以及map_server节点发布的topic名称均默认为"/map"，因此，要想在Cartographer建图的同时显示本地地图背景，需要在开始动态建图之前对map_server节点发布的Map话题名进行修改，这里可以借助rosbag工具包进行，具体方法如下：

1.使用map_server节点回放地图数据：

 rosrun map_server map_server lidar_map.yaml _frame_id:=map_lidar

2.使用rosbag record工具将map数据（topic：/map）记录在rosbag中：

rosbag record /map

由于本地yaml文件只会保存一帧地图数据，因此上述record工具不需要执行太长时间，执行完成后，利用rosbag info工具查看保存的rosbag信息如下所示：

3. 回放记录了地图信息的rosbag
使用rosbag play 命令回放含有地图信息的rosbag，并将原topic名称“/map”修改为其他名称，例如“/map_lidar”,示例如下：

rosbag play 2019-12-22-17-00-03.bag /map:=/map_lidar --loop    #/map是原topic，/map_lidar是新topic

其中，–loop选项指定数据包循环回放。

可以通过rostopic list命令看到新增了名为map_lidar的topic：

4.利用cartographer_ros动态建图
具体建图步骤，此处不再赘述，待建图程序开启rviz后，可以参考本文第五节“在rviz上显示回放的地图数据”中介绍的方法，在rviz中添加显示话题为“/map_lidar”的地图数据。

5.添加map_lidar到当前地图坐标系的坐标转换关系
如前所述，话题/map_lidar所处坐标系（frame_id）为"map_lidar"，我们可以在ros的launch文件中添加坐标系“map_lidar”到当前地图坐标系（以“map_laser”为例）的转换关系，相应的launch文件内容如下：

<launch>
    <node pkg="tf" type="static_transform_publisher" name="corrdinate_map_lidar_background" args="5.0 -2.5 0.0 1.47 0.0 0.0 map_laser map_lidar 1000" />
</launch>

该launch文件调用了ros的tf功能包中的static_transform_publisher节点，用来指定由父坐标系map_laser到子坐标系map_lidar所经过的变换，具体参数信息参考下面的说明：

static_transform_publisher x y z yaw pitch roll frame_id child_frame_id period_in_ms

另外，我们还可以将上述第3步和第5步执行的内容添加到建图程序启动的launch文件开头，即可实现建图程序的一键启动。launch文件中添加的内容如下：

  <node   
                pkg =  "rosbag"  
                type =   "play"  
                name =  "play"
                args =  "$(find cartographer_ros)/bag/2019-12-22-19-05-25_map_lidar.bag /map:=/map_lidar
                --loop">
  </node>

  <include file="$(find cartographer_ros)/launch/tf_shangqi_map_lidar_background.launch" />

添加了地图背景之后的建图程序显示效果如下图所示：