上一節我們介紹了wpr_simulation這個開源項目的簡單使用，這一節我們繼續深入，在Gazebo裡進行SLAM建圖和Navigation導航的仿真。
一、代碼更新
項目wpr_simulation的代碼為這次實驗進行了更新，需要重新下載。如果之前有下載了老版本的代碼，可以進入~/catkin_ws/src，直接刪除wpr_simulation文件夾，然後按照下面操作下載新的代碼。
從Ubuntu桌面左側的啟動欄裡點擊「Terminal」終端圖標，啟動終端程序（也可以通過同時按下鍵盤組合鍵「Ctrl + Alt + T」來啟動），輸入如下指令下載項目源碼：
其中「catkin_ws」為ROS的工作空間，請根據電腦的環境設置進行修改。這個項目的源碼是從Github網站下載的，所以下載過程需要連接網際網路。
然後是啟智ROS機器人的源碼包wpb_home，這個項目如果之前已經下載過，這次不用更新，使用以前的代碼也可以。
所有源碼下載完成後，運行如下指令進行源碼工程的編譯：
這個開源工程，用到了Gazebo，如果之前安裝的ROS是完整版本，則會很順利的編譯通過。如果編譯過程中提示缺少某些依賴項，可以通過如下指令補充完整：
二、SLAM原理簡介
在進行具體實驗之前，我們先簡單介紹一下原理。「SLAM」，英文全稱是「Simultaneous Localization And Mapping」，翻譯過來就是「即時定位與地圖構建」。SLAM最早由Smith、Self和Cheeseman於1988年提出，由於其重要的理論與應用價值，被很多學者認為是實現真正全自主移動機器人的關鍵。要理解SLAM，先得理解雷射雷達的數據特點，雷射雷達的掃描數據可以理解為一個障礙物分布的切面圖，其反映的是在一個特定高度上，障礙物面向雷達的面的邊緣形狀和分布位置。
所以，當攜帶雷射雷達的機器人在環境中運動時，它在某一個時刻，只能得到有限範圍內的障礙物的部分輪廓和其在機器人本體坐標系裡的相對位置。比如在下圖中，反映了一個機器人在相鄰比較近得A、B、C三個位置雷射雷達掃描到的障礙物輪廓。
雖然此時我們還不知道位置A、B、C的相互關係，但是通過仔細觀察，可以發現在A、B、C三個位置所掃描到的障礙物輪廓，某一些部分，是可以匹配重合的。因為這三個位置離得比較近，我們就假設掃描到的障礙物輪廓的相似部分就是同一個障礙物，這樣就可以試著將相似部分的障礙物輪廓疊加重合在一起，得到一個更大的障礙物輪廓圖案。比如位置A和位置B的障礙物輪廓疊加後如下：
再比如，位置B和位置C的障礙物輪廓疊加後如下：
按照上述的方法，將連續的多個位置雷射雷達掃描到的障礙物輪廓拼合在一起，就能形成一個比較完整的平面地圖。這個地圖是一個二維平面上的地圖，其反映的是在雷射雷達的掃描面上，整個環境裡的障礙物輪廓和分布情況。在構建地圖的過程中，還可以根據障礙物輪廓的重合關係，反推出機器人所走過的這幾個位置之間的相互關係以及機器人在地圖中所處的位置，這就同時完成了地圖構建和機器人的自身實時定位這兩項功能，這也就是「SLAM」全稱「Simultaneous Localization And Mapping」的由來。同樣以前面的A、B、C三個位置為例，將三個位置的雷射雷達掃描輪廓拼合在一起，就能得到一個相對更完整的平面地圖，同時也得出A、B、C三個位置在這個地圖中的位置：
ROS支持多種SLAM算法，其中主流的是Hector SLAM和Gmapping，其中Hector SLAM僅依靠雷射雷達就能工作，其原理和前面描述的方法比較類似；Gmapping則是在上述方法的基礎上，還融合了電機碼盤裡程計等信息，其建圖的穩定性要高於Hector SLAM。在這一節實驗裡，我們主要使用Gmapping。
三、SLAM建圖的仿真
我們先進行SLAM建圖的仿真。在這個仿真中，我們將使用Gmapping算法，需要使用USB手柄來控制機器人在場景中進行移動，遍歷所有活動區域，建立環境地圖。
將手柄接到電腦USB上之後，通過如下指令啟動SLAM建圖的仿真場景
啟動後，會彈出Gazebo窗口，裡面顯示的是一個模擬RoboCup@Home比賽環境的場景。場景中共有四個房間，分別為客廳、臥室、餐廳和廚房，在每個房間裡都放置了一些床、柜子之類的家具。這個場景還有兩個出入口，我們的機器人初始位置就位於靠近客廳的這個入口。
我們建圖的效果，主要是在Rviz裡進行觀察。在Ubuntu的左側任務欄裡，可以看到Rviz的程序圖標，用滑鼠點擊將Rviz界面喚回前臺顯示。
在Rviz裡，可以看到機器人面前大片的地面基準都是深灰色，只有機器人腳下出現一片白色圖案。這個圖案，是由很多條線段疊加而成，這些線段是機器人本體中心地面投影和每一個雷射雷達紅色障礙點的連線，也就是測距雷射飛行的軌跡，表示這條線段內部沒有障礙物。
我們可以使用USB手柄遙控機器人移動，在場景裡巡遊，Gmapping會調用內部的SLAM算法，把整個場景的地圖都掃描出來。
如果沒有USB手柄怎麼辦？沒關係，這裡準備個程序，可以使用鍵盤控制機器人完成巡遊移動。在Ubuntu裡再打開一個終端，輸入如下指令：
回車執行後，會提示控制機器人移動對應的按鍵列表。需要注意的是，在控制過程中，必須讓這個終端程序位於Gazebo窗口前面，處於選中狀態。這樣才能讓這個終端程序能夠持續獲得按鍵信號。
用鍵盤控制機器人移動的時候，只需要點一下鍵盤按鍵就可以讓機器人沿著對應方向移動，不需要一直按著不放，必要的時候使用空格鍵剎車。機器人在場景裡巡遊一遍之後，可以看到建好的地圖如下：
我們把地圖保存下來，後面進行自主導航時會用到。保存地圖時，需要保持Gmapping的程序仍在運行，不能關閉建好圖的Rviz界面。啟動一個終端程序，輸入以下指令：
按下回車鍵，確認保存。
保存完畢後，會在Ubuntu系統的「主文件目錄」裡生成兩個新文件，一個名為「map.pgm」，另一個名為「map.yaml」，這就是我們使用Gmapping建好的環境地圖。現在我們可以關閉Gazebo和Rviz窗口，準備進行導航的仿真。
SLAM建圖仿真視頻
四、Navigation導航仿真
首先，將「主文件目錄」裡的「map.pgm」和「map.yaml」兩個文件都拷貝到工作目錄「catkin_ws/src/wpr_simulation/maps」裡:
這個複製操作也可以在「文件管理器」裡用滑鼠完成：
地圖文件放置完畢，輸入以下指令啟動導航仿真：
按下回車鍵，系統會啟動Gazebo窗口，可以看到機器人又回到初始的那個入口。
在Ubuntu的左側任務欄裡，可以看到Rviz的程序圖標，用滑鼠點擊將Rviz界面喚回前臺顯示。
在Rviz窗口中，可以看到機器人位於我們剛才建好的地圖當中。
再仔細看看此時的地圖，在原來的黑色圖案（靜態障礙物輪廓）的周圍，出現了藍色的色帶，這個色帶表示的是安全邊界，色帶寬度和機器人的半徑大致相等。也就是說如果機器人進入這個色帶，就有可能和靜態障礙物（牆壁或桌椅腿）發生碰撞，這個安全邊界在後面機器人規劃路徑時會用到。
另外一個可以注意到的，Rviz中機器人的當前位置在地圖中央，這個和實際機器人位置不符。我們需要在導航前將機器人設置到正確的位置，點擊Rviz界面上方工具欄條裡的「2D Pos Estimate」按鈕：
然後再點擊Rviz的地圖裡，現實機器人所處的位置。這時，會出現一個綠色大箭頭，代表的是機器人在初始位置的朝向。按住滑鼠鍵不放，在屏幕上拖動畫圈，可以控制綠色箭頭的朝向。
在Rviz中拖動綠色箭頭，選擇好朝向，鬆開按鍵，機器人模型就會定位到我們選擇的位置。
調整虛擬機器人的初始位置，直到紅色的雷射雷達數據點和靜態障礙物的輪廓大致貼合。設置好機器人的初始位置後，可以開始為機器人指定導航的目標地點。點擊Rviz界面上方工具條裡的「2D Nav Goal」按鈕：
然後點擊Rviz裡地圖上的導航目標點（通常在白色區域裡選擇一個地點）。此時會再次出現綠色箭頭，和前面的操作一樣，按住不放在屏幕上拖動畫圈，設置機器人移動到終點後的朝向。
選擇完目標朝向後，鬆開點擊，全局規劃器會自動規劃出一條紫色的路徑，這條路徑從機器人當前點出發，繞開藍色的安全邊界，一直到移動目標點結束。
路徑規劃完畢後，機器人會開始沿著這條路徑移動，此時切換到Gazebo窗口，可以看到仿真環境裡的機器人也開始沿著這條路徑移動。
機器人到終點後，會原地旋轉，調整航向角，最終朝向剛才設置目標點時綠色箭頭的方向。
Navigation導航仿真視頻
五、用代碼控制機器人進行導航
ROS中的Navigation導航過程是可以通過代碼來控制的，這裡有個簡單的導航程序可以供我們參考，它的原始碼位置：
我們可以用Visual Studio Code之類的IDE打開這個源碼文件：
(1) 代碼的開始部分，先include了三個頭文件，第一個是ros的系統頭文件，第二個是導航目標結構體move_base_msgs::MoveBaseGoal的定義文件，第三個是actionlib::SimpleActionClient的定義文件。
(2) ROS節點的主體函數是int main(int argc, char** argv)，其參數定義和其他C++程序一樣。
(3) main函數裡，首先調用ros::init(argc, argv, "demo_simple_goal");進行該節點的初始化操作，函數的第三個參數是節點名稱。
(4) 接下來聲明一個MoveBaseClient對象ac，用來調用和主管監控導航功能的服務。
(5) 在請求導航服務前，需要確認導航服務已經開啟，所以這裡調用ac.waitForServer()函數來查詢導航服務的狀態。ros::Duration()是睡眠函數，參數的單位為秒，表示睡眠一段時間，這段時間若被某個信號打斷（在這個例程裡，這個信號就是導航服務已經啟動的信號），則中斷睡眠。所以ac.waitForServer(ros::Duration(5.0));的意思就是：休眠5秒，若期間導航服務啟動了，則中斷睡眠，開始後面的操作。用while循環來嵌套，可以讓程序在休眠5秒後，若導航服務未啟動，則繼續進入下一個5秒睡眠，直到導航服務啟動才中斷睡眠。
(6) 確認導航服務啟動後，我們聲明一個move_base_msgs::MoveBaseGoal類型結構體對象goal，用來傳遞我們要導航去的目標信息。goal.target_pose.header.frame_id表示這個目標位置的坐標是基於哪個坐標系，例程裡賦值「map」表示這是一個基於全局地圖的導航目標位置。goal.target_pose.header.stamp賦值當前時間戳。goal.target_pose.pose.position.x 賦值-3.0，表示本次導航的目的地是以地圖坐標係為基礎，向X軸反方向移動3.0米。goal.target_pose.pose.position的y賦值2.0，表示本次導航的目的地是以地圖坐標係為基礎，向Y軸正方向移動2.0米。goal.target_pose.pose.position的z未賦值，則默認是0；goal.target_pose.pose.orientation.w 賦值1.0，則導航的目標姿態是機器人面朝X軸的正方向（正前方）。
(7) ac.sendGoal(goal);將導航目標信息傳遞給導航服務的客戶端ac，由ac來監控後面的導航過程。
(8) ac.waitForResult();等待MoveBase的導航結果，這個函數會保持阻塞，就是卡在這，直到整個導航過程結束，或者導航過程被其他原因中斷。
(9) ac.waitForResult()阻塞結束後，調用ac.getState()獲取導航服務的結果，如果是「SUCCEEDED」說明導航順利到達目的地，輸出結果「Mission complete!」；若不是這個結果，輸出結果「Mission failed ...」。
下面試著在我們的仿真環境裡運行這個程序。首先和剛才一樣，先通過下面的指令打開仿真環境：
在Ubuntu的左側任務欄裡，可以看到Rviz的程序圖標，用滑鼠點擊將Rviz界面喚回前臺顯示。通過Rviz界面上方工具欄條裡的「2D Pos Estimate」按鈕將機器人設置到地圖的入口處。
下面運行demo_simple_goal程序，驅動機器人完成導航任務。啟動終端程序，輸入以下指令：
按下回車鍵後，可以看到demo_simple_goal節點發出導航服務請求的提示「Sending goal」。
此時再查看Rviz的顯示界面，可以看到一條紫紅色的線條從機器人腳下延伸到目標點，路徑規劃成功。
切換到Gazebo窗口，可以看到機器人沿著規劃的路徑，緩慢移動到目標點。
機器人到達目標點後，可以看到顯示順利到達導航目的地的信息「Mission complete!」
六、仿真場景的改變
這個實驗的場景裡的家具是可以進行移動調整的，在Gazebo窗口的工具欄裡，點擊「移動」圖標。
然後再點擊場景裡的任意物體（家具），會看到物體身上出現了xyz三個軸的正方向箭頭。點擊其中任意一個方向，按住滑鼠拖動，就能夠移動這個物體。
場景布局改變後，我們可以再次進行建圖和導航的仿真，對比不同環境下的運行效果。
七、問題反饋
至此，關於這個仿真項目的建圖導航部分就介紹完了，如果同學們在安裝和使用過程中遇到問題，可以在這個項目的Github主頁上提交issuse，我們會及時回復，讓其他遇到類似問題的小夥伴也能看到。提交issuse的方法：
(1) 在瀏覽器中打開項目的Github主頁：
https://github.com/6-robot/wpr_simulation
(2) 點擊分頁欄的第二項「Issues」切換頁面：
(3) 在「Issues」頁面中點擊「New issue」即可提交新的issuse：
八、結尾
這一節我們只是使用Navigation系統完成了一個導航任務，下一節我們會深入剖析ROS的Navigation系統，讓同學們能詳細了解整個導航過程中都是那些模塊參與了工作，它們之間的協作關係又是如何。如果你喜歡這個項目，麻煩不要吝嗇手中的滑鼠，賞給我們一顆小星星！您的滿意是我們持續更新的最大動力！謝謝~~我們下次再見！(繼續比心)