基於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" />

添加了地圖背景之後的建圖程序顯示效果如下圖所示：