第三章 使用ROS任務執行
正如我們在卷一講述的，機器人的程序相對簡單來運行一個特定的行爲，例如人臉跟蹤、位置導航、或跟隨一個人。但是一個期望的完全自主機器人從一個巨大的行爲列表中選擇一個自己的行爲取決於手頭的任務和當前的條件。
在本章節中，我們將學習如何使用兩個不同啓用ROS任務執行框架：SMACH（使用狀態機和宣言“smash”）和pi_trees（行爲樹）。每一種方法都有其有點和缺點，並且一個是否容易取決於你的編程背景。但相比於寫一個巨大的if-then語句列表來說，這兩種方式都提供了一個更結構化的方法進行任務管理。
整個任務控制器通常被稱爲任務執行，並且大多數這樣期望的控制器至少包括以下主要特點：
- l 任務優先級：如果一個更高的優先級任務要求同樣的資源（例如驅動電機），一個低的優先級任務應該等待。
- l 暫停和恢復：當一個高優先級的給定任務是控制，那麼通常需要暫停當前運行的任務（或多個任務），然後當進程返回控制時，應當恢復被搶佔(preempted)的任務。例如Neato吸塵器在充電完成後，能夠返回離開的位置。
- l 任務層次結構：任務通常可以分解成子任務來處理細節。例如，一個稱爲再充電的高級任務可能包含三個子任務：導航到充電樁、對接機器人和插上電池充電。同樣的，對接任務可以分解成：匹配信標、驅動前進、對接停止。
- l 條件：傳感器數據和內部編程變量可以限制何時以及如何執行一個任務、在ROS中，這些變量通常通過各個節點以消息的形式發佈。例如，電池監控任務訂閱一個診斷話題，該話題包含當前電池狀態。當檢測到電池電量過低時，一個檢查電池條件應該被觸發並暫停或終止其他任務，執行再充電任務。
- l 併發性：多個任務可以並行。例如，導航任務（到下一個地點）和電池監控任務必須同時運行。
爲了保持這個整體，我們使用兩個貫穿章節的示例場景：一個“巡邏機器人”必須按順序訪問一系列位置，同時保持電池檢測。和一個“家庭清潔機器人”必須訪問一組房間，並且在每一個房間執行各種清潔相關的任務。
3.1 虛擬電池仿真
一個永遠運行的機器需要監控它自己的電池狀態，並且必要時再充電。爲了使我們的示例更真實，因此，我們將使用一個節點來模擬一個電池，通過在一個電池狀態ROS話題上發佈一個實時減少的值。其他節點可以訂閱這個話題並且當電池電量過低時可以做出反應。
電池模擬節點battery_simulator.py可以在rbx2_utils/nodes目錄下找到。這個腳本相當簡單除了部分設計到的動態重新配置，關於細節我們將在第7章詳細介紹。現在，我們只需要注意以下：
這個節點需要三個參數：
- l rate：（默認1Hz）——多久發佈電池狀態
- l battery_runtime：（默認60秒）——電池需要多少秒減少到0
- l initial_batery_level：（默認100）——當開始運行節點時，初始化電池容量。
節點使用initial_batery_level這個浮點型開始在/battery_level話題發佈，隨後通過給定battery_runtime參數在一定時間內計數到0。正如我們在battery_simulator.launch中的那樣，這些參數可以被指定，該launch文件可以在rbx2_utils/launch目錄下找到。你也可以在命令行作爲一個參數指定電池運行時間（單位秒）。讓我們現在通過運行launch文件來測試模擬器：
$ roslaunch rbx2_utils battery_simulator.launch你可以通過打開另一個終端然後運行以下命令來驗證模擬器：
$ rostopic echo /battery_level類似的輸出如下：
data: 91.0
---
data: 90.6666641235
---
data: 90.3333358765
--- 等等
這個節點也定義了一個ROS服務，稱爲set_battery_level需要一個浮點型數值作爲一個參數輸入，並且會設置電池容量。我們將使用這個服務來模擬再充電通過設置等級到100或設置一個很低的值來模擬電池突然耗盡。
set_battery_level服務可以使用以下方式：
$ rosservice call /battery_simulator/set_battery_level 100參數範圍是0到100。模擬再充電，使用set_battery_level服務來設置等級到一個高的數字例如100.。，模擬突然耗盡電池，設置這個值到一個很低的數字例如30。這將允許我們測試各個節點如何應對低電池狀態。
電池模擬器也可以使用rqt_reconfigure來控制。爲了實時電池充電或手動設置電池等級，啓動rqt_reconfigure：
$ rosrun rqt_reconfigure rqt_reconfigure然後點擊battery_simulator節點來設置以下選項：
使用滑塊或文本框來改變電池運行時或電池容量。
在這一章我們使用電池模擬器來比較不同的任務框架是如何使我們能夠處理較低的電池狀態的同時執行其他任務。
3.2 運行例程的公共設置
我們例程的所有代碼分享了一個公共設置。將會有四個座標點（目標位置）分佈在廣場的角落，相距爲1米。充電樁位於廣場的中心。所有的座標點和充電樁將會在Rviz作爲可視化標記進行顯示。其中座標點是顏色方塊，充電樁爲黃色圓盤。這些標記並沒有深度，所以機器人可以自由通過。基本設置如下所示，使用的是虛擬TurtleBot。
這些變量的設置通過task_setup.py設置，該文件位於rbx2_tasks/src/rbx2_tasks目錄下。在每一個例子中，我們將導入這個文件來配置基本環境。一些更加重要的變量在task_setup.py文件中設置，如下：
- l squre_size（默認：1.0米）
- l low_battery_threshold（默認：50）
- l n_patrols（默認：2）
- l move_base_timeout（默認：10秒）
我們也可以定義座標點和充電樁的位置。任何這些都隨你的需要而可以改變。
最後，我們定義一個move_base客戶端和一個cmd_vel發佈器用於控制機器人運動。
3.3 簡單回顧ROS行爲
在本章節中，由於我們需要使用相當多的move_base行爲，因此是一個好的方式來回顧ROS的行爲概念。一定要從ROS Wiki的actionlib概述開始，然後使用在線教程提供的C++和Python教程。
回想一下ROS行爲期望行爲客戶端提交的一個目標。行爲服務器通常會提供目標的反饋進展和一個目標是否成功(succeed)、崩潰或搶佔(preempted)的結果。
也許ROS行爲最熟悉的例子是導航堆棧裏的MoveBaseAction。move_base包實現了一個行爲服務器來接收一個目標機器人的位姿（位置和方向）和試圖到達的目標，到達目標過程通過發佈Twist消息的同時監視里程計和激光掃描數據來避障。在這個過程中，反饋將以一個時間戳的位姿的形式提供，該形式對應機器人狀態，以及目標狀態（例如ACTIVE、SUCCEEDED、ABORTED等）。行爲結果只是一個帶有時間戳的狀態消息，用於表明到達目標成功(succeed)或崩潰或搶佔(preempted)等。
你可以使用命令來查看MoveBaseAction的完整定義：
$ rosmsg show MoveBaseAction查看反饋消息語法，使用命令：
$ rosmsg show MoveBaseActionFeedback並且查看結果可能返回狀態列表，運行命令：
$ rosmsg show MoveBaseActionResult回想一下卷一，我們通過使用一系列的move_base行爲來編程我們的機器人進行方塊導航。對於廣場的每一個角落，我們發送對應的位姿到move_base行爲服務器，然後提交下一個目標位姿之前等待結果。然而，虛擬設機器人在座標點之間移動，我們也想機器人在每一個位置運行一組子任務。例如，一個任務可能會尋找一個特定的對象，隨後記錄其位置或如果機器人有一個手臂和手來進行撿起實驗。與此同時，我們想讓機器人檢測電池容量並導航到固定位置等等。
所有的這些可以通過ROS的行爲來實現，但其中不得不爲每一個任務創建一個行爲服務器，然後使用一組if-then條件來協調任務或在行爲之間進行回調。當然是可能的，結果將會相當乏味。幸運的是，SMACH和行爲樹會幫助將這些更復雜的情況變的更容易。
3.4 巡邏機器人示例
虛擬設我們的機器人任務是在一個廣場巡邏，通過按順序的從一個角落到另一個角落的形式。如果電池電量低於設定的閾值時，機器人將停止巡邏而導航到充電樁。再充電完成後，機器人將回到離開的地方繼續巡邏。
基本的巡邏任務看起來像這樣：
- l 初始化：
- n 設置目標點
- n 設置回充座標點
- n 設置需要巡邏的數字
- l 任務（遵循優先級）：
- n CHECK_BATTERY
- n RECHARGE
- n PATROL
- l 傳感器和執行器：
- n 電池傳感器；激光掃描儀；RGB-D相機等等。
- n 驅動電機
CHECK_BATTERY任務是簡單設置一個標記，當電源電量等於設置的閾值時。
RECHARGE任務可以分解成以下幾個子任務：
RECHARGE：NAV_DOCK→ CHAGE
NAV_DOCK意味着導航到充電位置。
PATROL任務可以分解成以下導航子任務序列：
PATROL：NAV_0 → NAV_1 → NAV_2→ NAV_3
這裏的每一個導航任務的下座標通過座標數字（每一個廣場的角落）給定。導航任務可以使用標準ROS MoveBaseAction目標實現，並且導航棧正如我們在卷一做過的。
在開始學習如何使用SMACH或行爲樹實現巡邏機器人之前，讓我們回顧下它怎樣使用一個標準的腳本完成。
3.5 巡邏機器人使用標準腳本
我們的腳本將訂閱電池等級話題，並帶有一個回調函數，如果等級下降到我們給定的閾值時，該回調函數設置一個low_battery標誌爲True。
def battery_cb(self, msg):
if msg.data <self.low_battery_threshold:
self.low_battery = True
else:
self.low_battery = False
當接收到電池容量話題內的值時，這個檢查會以同樣的頻率觸發。
與此同時，我們的主控制循環有可能開始如下查看：
while n_patrols < max_patrols:
if low_battery:
recharge()
else:
patrol()
每一個訓練開始，我們檢查電池等級，如果有必要進行回充。否則，我們開始巡邏，當然，這是簡單的策略用於練習，由於當機器人巡邏是一個過程，電池有可能在兩次檢查過程中沒電。讓我們看看如何正確解決這個問題。
patrol()例程通過一組座標序列移動機器人，看起來像這樣：
def patrol():
for location in waypoints:
nav_to_waypoint(location)
當我們以這樣的形式編寫的時候，我想我們應該移動電池檢查到nav_to_waypoint()函數：
def nav_to_waypoint(location):
if low_battery:
recharge()
else:
move_to(location)
至少我們現在可以在移動到每一個座標點之前檢查電池等級。然而，move_to(localtion)函數可能會話一些時間來處理，取決於到下一個座標點有多遠。因此我們真正需要移動電源檢查更深一些，並且放到move_to()進程中。
在ROS中，move_to()函數可能調用MoveBaseAction服務器實現，因此對於move_base客戶端來說，電池檢查應該在反饋回調函數中完成。結果看起來像這樣：
move_base.send_goal(goal, feedback_cb=self.nav_feedback_cb)
def nav_feedback_cb(self, msg):
if self.low_battery:
self.recharge()
現在我們從MoveBaseAction服務器接收反饋消息來檢查電池狀態，這樣可以有足夠的頻率來避免電池沒電。recharge()函數會在發送一個新目標到MoveBaseAction服務進行導航之前取消當前move_base目標，從而選擇導航機器人到充電位置來進行充電。
全部的代碼可以在patrol_script.py中找到，該文件位於rbx2_tasks/nodes目錄下。
源碼鏈接：patrol_script.py
腳本是相當直觀的，並且不會詳細描述。不過，你可以對其進行測試如下。
首先通過fake_turtlebot.launch文件在ArbotiX模擬器中啓動虛擬Turtlebot，該launch文件位於rbx2_tasks/launch目錄下。這個文件將啓動虛擬Turtlebot、一個配有空白地圖的move_base行爲服務器和一個默認運行時間爲60秒的電池模擬器節點：
$ roslaunch rbx2_tasks fake_turtlebot.launch接下來，啓動帶有nav_tasks.rviz配置文件的RViz：
$ rosrun rviz rviz -d `rospack find rbx2_tasks`/nav_tasks.rviz最後，運行patrol_script.py腳本：
$ rosrun rbx2_tasks patrol_script.py在RViz中，界面應該如下所示：
機器人應當在廣場周圍執行兩次循環，同時檢測電池水平。每當電池低於腳本定義的閾值（50）時，機器人將運動到廣場中間中間的圓形進行充電。一次充電（電池級別設置會100），機器人應該到離開的地方繼續巡邏。例如，如果當它完成充電後，沒有到第二個爲座標，它將在繼續循環之前返回到第一個座標點。