【ROS書籍】ROSByExampleⅡ——第三章使用ROS任務執行 (二)

3.6 腳本的問題

以上描述的標準腳本存在的主要問題是，我們不得不必須將電池檢查深埋在導航程序。同時這個工作的定義用於示例聯繫，當我們給機器人行爲添加更多的任務時，將變得不那麼有效。例如，虛擬設我們想要機器人導航到下一個位置之前，在每一個座標點通過左右平移相機掃描一個人時。我們的巡邏程序可能會是這樣：

def patrol():
  for location in waypoints:
    nav_to_waypoint(location)
    scan_for_humans()

同時相機會向後旋轉，我們仍需要密切關注電池等級，但現在我們不會運行move_base目標。這意味着我們需要添加第二個電池檢查，這個時候頭部平移。如果電池低於我們設定的閾值，我們不需要取消導航目標；相反，在移動到充電站之前，我們需要重新定位相機。

對於每一個額外的任務，我們需要添加另一個電池檢查，這將會造成我們的代碼冗餘，使我們的總體任務執行更模塊化。從概念上講，電池檢查應該在我們的任務層次結構的頂部，我們真的應該在每一個傳遞任務列表過程中進行電池檢查。

腳本方法的另一個缺點是，我們是直接調用ROS行爲、話題和服務，而不是通過可重用的包裝器來隱藏常見細節。例如，除了話題訂閱電池水平，很有可能我們還將訂閱其他話題所產生的額外的傳感器，如深度相機、碰撞傳感器或激光掃描儀。所有這些用戶共享一個類似的話題名稱、話題類型和回調函數的設置模式。任務架構像SMACH定義ROS話題、服務和行爲的封裝照顧到了設置細節從而允許從一個給定的任務執行添加或刪除這些對象，而無需重複無數行代碼。

3.7 SMACH或行爲樹？

SMACH是一個Python庫用於編譯複雜的機器人行爲，使用的是層次狀態機。smach_ros包提供了緊密集成的ROS話題、服務和行爲，並且有一組SMACH教程涵蓋了所有的功能。選擇這個的原因是，對於大多數ROS初學者，SMACH是一個好的方式。然而，如果你還沒有熟悉有限狀態機，這個方法或許會有一些疑惑。如果你卡住了，這裏還有其他可選項適合你。

一個相對較新的方法稱爲行爲樹，首先流行在程序員的電腦遊戲。最近，行爲樹也在機器人中使用。我們將使用一個pi_trees包，該包作者專門爲本書寫的，當然你也可以使用任何你認爲合適的方式。行爲樹組織機器人任務樹結構，使它們相對容易概念化和程序化。此外，許多我們需要的任務執行的屬性如優先級(包容)、暫停和恢復(搶佔(preempted))、分層結構(子任務)和條件檢查是行爲樹的自然屬性，一旦我們建立基礎設施，那麼我們很容易得到“免費”。

我們也應該提到的第三種方法稱爲teer。Teer也是一個獨立的Python庫用於協調任務，但它使用協同程序而不是狀態機。協同程序就像一個子程序，但它運行時可以訪問其內部狀態。(在Python中，這是通過使用yield關鍵字。)因此，協同程序可以用來表示同時運行的任務也可以暫停和重新開始。Teer沒有內置ROS話題、服務和動作的封裝包，像SMACH或pi_trees需要更多工作起來和在ROS環境中運行；因此；我們不會在本卷講述。

3.8 SMACH：任務作爲狀態機

在開始使用SMACH之前，確保你已經安裝了必要的ROS包：

$ sudo apt-get install ros-indigo-smach ros-indigo-smach-ros \ rosindigo-executive-smach ros-indigo-smach-viewer

在一些我們的例程中，我們也需要一個稱爲python-easygui的簡單GUI包，讓我們現在安裝它：

$ sudo apt-get install python-easygui

SMACH使我們能夠使用有限狀態機編寫行爲序列。顧名思義，一個狀態機可以在給定的時間切換成任何states。機器還接受輸入(inputs)，根據輸入的值和當前狀態，機器可以轉換(tramsition)到另一個狀態。作爲一個轉型的結果，狀態機也可能產生的結果(outcome)。有限狀態機也稱爲自動機(automata)或反應系統(reactive systems)，因爲我們只需要知道機器的當前狀態和輸入來預測下一個行爲。

最簡單的一個狀態機的實際例子是一把鎖和鑰匙。鎖可以在有兩種狀態：鎖定(LOCKED)或解鎖(UNLOCKED)，同時鑰匙順時針或逆時針方向輸入。(按照慣例，狀態名稱全部用大寫字母寫。)可能的狀態轉換是鎖定(LOCKED)，解鎖(UNLOCKED)和根據鑰匙的旋轉方向轉變狀態從解鎖(UNLOCKED)到鎖定(LOCKED)。(出於完整性的考慮，我們也可以指定鎖定(LOCKED)到鎖定(LOCKED)，解鎖(UNLOCKED)到解鎖(UNLOCKED)間的轉換)。結果是，門可以打開，或者不開。

SMACH包提供了一個獨立的Python庫用於創建狀態機和ROS封裝用於集成ROS話題、服務和行爲庫。SMACHWiki頁面包含許多優秀的教程，並且鼓勵讀者通過儘可能多方式去工作。至少，它是必不可少的理解開始頁面。我們將承擔一些熟悉的教程作爲我們的示例。

SMACH有很多特性並且看起來有點壓倒性。所以我們將依次使用示例演示每個組件來提供一些視覺反饋。但首先，一個簡短的回顧在線教程中你學到了什麼。

3.8.1 SMACH回顧

我們虛擬設你已經通過至少部分SMACH Wiki上的教程，所以我們只能提供基本概念回顧。如果你需要一個給定的類或函數的更多細節，你可以查SMACHAPI。你也可以直接在找GitHub源。

SMACH狀態(states)是Python類，擴展smach.State類通過重寫execute()方法返回一個或多個可能的結果(outcomes)。execute方法也可以用一個可選的參數定義用戶數據(userdata)的集合，可用於狀態之間的傳遞信息。執行的實際計算狀態可以是任何你想要的，但是有許多預定義的狀態類型，可以節省很多不必要的代碼。特別是，SimpleActionState類將一個常規的ROS行爲(action)變成一個SMACH狀態。同樣，MonitorState封裝了一個ROS話題而且ServiceState處理ROS服務。CBState使用@smach.cb_interface裝飾將幾乎任何你喜歡的功能變成SMACH狀態。

SMACH狀態機(state machine)是另一個Python類(smach.StateMachine)可以包含很多狀態。添加一個狀態機狀態通過定義一組從狀態的結果(outcomes)到其他狀態的轉換。當運行一個狀態機，這些轉換決定從狀態到狀態的流程：

input → STATE_1 → {outcome, transition}→ STATE_2

input → STATE_2 → {outcome, transition}→ STATE_3等等

一個狀態機本身必須有一個結果(outcome)，並且可以成另一個狀態機狀態。通過這種方式，狀態機可以嵌套層次結構。例如，狀態機稱爲“大掃除”可以包含嵌套的狀態機“真空客廳”，“拖把廚房”，“洗浴盆”等等。更高層次狀態機“大掃除”將決定嵌套的狀態機地圖到全局結果(如：“所有任務完成”或“並非所有的任務完成”)。

有許多預定義的SMACH容器也可以節省很多編程。Concurrence容器返回一個結果，結果取決於多個狀態和允許一個狀態搶佔(preempted)另一個狀態。Sequence順序容器自動在狀態之間生成順序轉換並添加它。Iterator容器允許你循環一個或多個狀態，直到滿足某些條件。我們將瞭解更多關於這些容器在我們需要他們的時候。

3.8.2 使用SMACH巡邏一個正方形

第一卷中，我們編程機器人導航在廣場使用各種方法包括move_base行爲。早些時候,在這一章裏,我們使用一個Python腳本做同樣的事情,同時監測模擬電池的水平。現在讓我們看看我們如何可以使用SMACH完成同樣的目標。

在本節中，我們將保留電池檢查和簡單的移動機器人廣場。在下一節中我們將加入電池檢查並在必要時使機器人充電。

概念化巡邏問題的一種方法是，我們想讓機器人處於四個狀態之一，即，提出了廣場的四個角落的定義。此外，我們希望機器人在一個特定的順序在這些狀態中實現轉換。等價於，我們可以說我們想讓機器人執行四個任務；也就是說，按序列形式運動到每一個角落位置。

讓我們以NAV_STATE_0到NAV_STATE_3的方式給四個狀態命名。我們的狀態機將被定義爲以下狀態和轉換：

NAV_STATE_0 →NAV_STATE_1
NAV_STATE_1 → NAV_STATE_2
NAV_STATE_2 → NAV_STATE_3
NAV_STATE_3 → NAV_STATE_0

這裏我們定義最後轉換帶我們回到起始狀態，因此整個狀態機是重複循環的。如果我們希望機器人在廣場最後位置停止一次，我們可以定義另一個狀態NAV_STATE_4，該狀態具有和NAV_STATE_0相同的目標姿態，然後改變上面最後轉換：

NAV_STATE_3 →NAV_STATE_4

我們可以終止機器（即停止機器人），通過添加最後一個轉換到空的狀態：

NAV_STATE_4 → ''

在SMACH中，狀態轉換依賴於以前的結果狀態。對於我們的機器人在位置之間的移動的情況，結果可以“成功(succeed)”，“崩潰(aborted)”或“搶佔(preempted)”。

這些想法在腳本的實現在patrol_smach.py，可以在rbx2_tasks/nodes目錄中找到。爲了節省空間，我們不會顯示整個代碼而是關注關鍵部分。(你可以點擊上面的腳本名來在線列出或自己的編輯器中打開腳本)。

在腳本頂部的import塊部分，我們需要引進SMACH對象來使用：

from smach import StateMachine
from smach_ros import SimpleActionState, IntrospectionServer

我們將需要StateMachine對象來創建整個狀態機，SimpleActionSate來封裝我們的調用到move_base和IntrospectionServer，這樣我們可以使用smach_viewer。

正如你從在線教程所知道的，SimpleActionState類型允許我們包裝一個常規ROS行爲到SMACH狀態。虛擬設我們已經分配角落到Python列表，該列表我們稱爲waypoints，代碼塊的這些姿態到簡單的行爲狀態是這樣的：

nav_states = list()
for waypoint in waypoints:
nav_goal = MoveBaseGoal()
nav_goal.target_pose.header.frame_id = 'map'
nav_goal.target_pose.pose = waypoint
move_base_state = smach_ros.SimpleActionState('move_base', MoveBaseAction,
goal=nav_goal, exec_timeout=rospy.Duration(10.0))
nav_states.append(move_base_state)

首先，我們創建一個空列表稱爲nav_states來持有我們的導航狀態，一個廣場的每個角落。接下來，我們遍歷每一個waypoints，創建一個標準MoveBaseGoal使用waypoint作爲理想的姿態。然後我們把這個目標變成SMACH狀態，使用聲明：

move_base_state = smach_ros.SimpleActionState('move_base', MoveBaseAction,
goal=nav_goal, exec_timeout=rospy.Duration(10.0))

我們使用了SimpleActionState狀態類型包裝MoveBaseAction動作狀態。SimpleActionState類的構造函數需要行爲話題名作爲第一個參數，行爲類型作爲第二個參數。它還支持關鍵字參數goal和exec_timeout用於指定行爲目標，我們願意等待它到達(以上一般情況是10秒)。最後，我們添加到nav_states列表狀態。

預定義的結果SimpleActionState有成功(succeed)，崩潰(aborted)或搶佔(preempted)。下一步是使用這些結果和狀態構建整個狀態機：

# Initialize the state machine
self.sm_patrol = StateMachine(outcomes=['succeeded','aborted','preempted'])
# Add the states to the state machine with the appropriate transitions with
self.sm_patrol:
StateMachine.add('NAV_STATE_0', nav_states[0],
transitions={'succeeded':'NAV_STATE_1','aborted':'NAV_STATE_1'})
StateMachine.add('NAV_STATE_1', nav_states[1],
transitions={'succeeded':'NAV_STATE_2','aborted':'NAV_STATE_2'})
StateMachine.add('NAV_STATE_2', nav_states[2],
transitions={'succeeded':'NAV_STATE_3','aborted':'NAV_STATE_3'})
StateMachine.add('NAV_STATE_3', nav_states[3],
transitions={'succeeded':'NAV_STATE_0','aborted':'NAV_STATE_0'})

首先我們用可能的結果“成功(succeed)”，“崩潰(aborted)”和“搶佔(preempted)”來初始化我們的巡邏狀態機。實際結果將取決於我們添加到狀態機的狀態，當運行時會提供結果。

接下來，我們添加每一個導航狀態到狀態機的狀態轉換字典中。每一行的第一個參數是我們分配到狀態的一個任意的名稱，因爲狀態轉換有引用到。按照慣例，這些狀態的名字用大寫字母明命名。例如，上面的第一行中添加狀態nav_states[0]到狀態機並且命名爲NAV_STATE_0。轉換的這個狀態會告訴我們，如果狀態成功(succeed)（機器人到達目標位置），我們希望下一個狀態是第二行定義的NAV_STATE_1，該狀態代表存儲的第二個目標姿態nav_states[1]。

注意我們如何映射結果“崩潰(aborted)”到下一個狀態。雖然這是可選的並且並不總是需要，它可以很好的和MoveBase目標運行，因爲由於障礙或時間的限制，基本路徑規劃可能不會總是成功(succeed)地讓機器人到當前目標。在這種情況下，結果會崩潰(aborted)，用簡單地停止機器人來代替，我們會繼續下一個目標。

還要注意最終狀態如何返回到第一個狀態轉換，NAV_STATE_0。在這種情況下，狀態機和機器人將圍繞廣場無限繼續循環。如果我們想讓機器人在第一次循環後停止，我們可以創建以下狀態機代替：

with self.sm_patrol:
  StateMachine.add('NAV_STATE_0', nav_states[0],
  transitions={'succeeded':'NAV_STATE_1','aborted':'NAV_STATE_1'})
  StateMachine.add('NAV_STATE_1', nav_states[1],
  transitions={'succeeded':'NAV_STATE_2','aborted':'NAV_STATE_2'})
  StateMachine.add('NAV_STATE_2', nav_states[2],
  transitions={'succeeded':'NAV_STATE_3','aborted':'NAV_STATE_3'})
  StateMachine.add('NAV_STATE_3', nav_states[3],
  transitions={'succeeded':'NAV_STATE_4','aborted':'NAV_STATE_4'})
  StateMachine.add('NAV_STATE_4', nav_states[0],
  transitions={'succeeded':'','aborted':''})

分配的最終狀態NAV_STATE_4具有和起點相同的姿勢狀態，我們將結果映射到空態從而終止狀態機和停止機器人。腳本patrol_smach.py實現狀態機的這個版本，但是在一個循環中執行，使我們能夠控制機器人完成巡邏的次數，正如接下來我們要展示的。

執行狀態機，我們使用循環：

while self.n_patrols == -1 or self.patrol_count < self.n_patrols:
  sm_outcome = self.sm_patrol.execute()
  self.patrol_count += 1
  rospy.loginfo("FINISHED PATROL NUMBER: " + str(self.patrol_count))

參數self.n_patrols在我們的task_setup.py中定義。該參數會返回它從ROS參數服務器中讀到的默認值3。(如果我們希望機器人永遠循環，我們可以使用特殊值-1。)計數器self.patrol_count也是在task_setup.py中的定義並且初始化爲0。

在每個經過循環，我們運行self.sm_patrol.execute()。execute()方法設置狀態機。如果機器終止，在以上最終轉換後會運行它，然後狀態機的總體結果將在變量sm_outcome中可用。

注意，不同於一般的腳本，我們不能簡單地放置一個while循環來圍繞我們狀態機的實際狀態，因爲這將導致通過狀態的construction在運行之前循環。

我們還將會添加以下兩行到我們腳本的大部分：

intro_server = IntrospectionServer('nav_square', self.sm_patrol, '/SM_ROOT')
intro_server.start()

SMACH自省(Introspection)(Introspection)服務器使我們能夠以圖形化smach_viewer的方式查看運行的狀態機，效果我們將在以下部分看到。

3.8.3 在ArbotiX模擬器中測試SMACH導航

讓我們使用ArbotiX模擬器試試patrol_smach.py腳本。

首先，運行rbx2_tasks包中fake_turtlebot.launch文件。這個文件將彈出一個虛擬TurtleBot，move_base行爲服務器與一個空白的地圖以及默認運行時60秒的虛擬電池模擬器，雖然我們不會在這個例子中使用電池：

$ roslaunch rbx2_tasks fake_turtlebot.launch

接下來，終止任何正在運行的RViz實例，然後和nav_tasks配置文件一起啓動它：

$ rosrun rviz rviz -d `rospack find rbx2_tasks`/nav_tasks.rviz

確保你可以在前臺看到RViz窗口，然後運行patrol_smach.py腳本：

$ rosrun rbx2_tasks patrol_smach.py

你應該看到機器人圍繞廣場移動三次，然後停止。在RViz中的視圖應該看起來像這樣：

同時，你應該在啓動patrol_smach.py腳本的終端看到下面的消息：

Starting Tasks

[INFO] [WallTime:1378991934.456861] State machine starting in initial state

'NAV_STATE_0'with userdata:[]

[WARN] [WallTime:1378991934.457513] Still waiting for action server 'move_base' to start... isit running?

[INFO] [WallTime:1378991934.680443] Connected to action server 'move_base'.

[INFO] [WallTime:1378991934.882364] Success rate: 100.0

[INFO] [WallTime:1378991934.882795] State machine transitioning

'NAV_STATE_0':'succeeded'-->'NAV_STATE_1'

[INFO] [WallTime:1378991940.684410] Success rate: 100.0

[INFO] [WallTime:1378991940.685345] State machine transitioning

'NAV_STATE_1':'succeeded'-->'NAV_STATE_2'

[INFO] [WallTime:1378991946.487312] Success rate: 100.0

[INFO] [WallTime:1378991946.487737] State machine transitioning

'NAV_STATE_2':'succeeded'-->'NAV_STATE_3'

[INFO] [WallTime:1378991952.102620] Success rate: 100.0

[INFO] [WallTime:1378991952.103259] State machine transitioning

'NAV_STATE_3':'succeeded'-->'NAV_STATE_4'

[INFO] [WallTime:1378991957.705305] Success rate: 100.0

[INFO] [WallTime:1378991957.705821] State machine terminating

'NAV_STATE_4':'succeeded':'succeeded'

[INFO] [WallTime:1378991957.706164] State Machine Outcome: succeeded [INFO]

[WallTime:1378991958.514761] Stopping the robot...

這裏我們看到SMACH報告當每個發生的狀態轉換，以及整體狀態機的最終輸出結果。這些行報告“success rate”是由我們的patrol_smach.py腳本創建的額外輸出，在下一節中我們將更詳細地檢查。

我們還可以通過smach_viewer.py工具看到一個狀態機的運行圖。想要打開查看器，需要打開另一個終端並運行：

$ rosrun smach_viewer smach_viewer.py

現在運行patrol_smach.py腳本：

$ rosrun rbx2_tasks patrol_smach.py

SMACH查看器中顯示應該是這樣的:

隨着機器人從狀態到狀態(即在廣場從位置到位置)，你應該在查看器中看到適當的高亮綠色的狀態。對於SMACH查看器GUI的完整描述，看smach_viewer Wiki頁面。

3.8.4 從SimpleActionState訪問結果

在我們當前的狀態機中，即使當前轉換崩潰，我們也可以決定移動到下一個狀態。但它可能是有用的來保持一個到達目標成功(succeed)的計數。例如，如果一個巡邏機器人並且成功(succeed)率低於某個閾值，那麼我們可能會懷疑這個機器人有問題或巡邏方式錯誤。

SMACH的SimpleActionState構造函數允許我們指定一個回調函數來獲得行爲結果。語法是這樣的：

move_base_state = SimpleActionState('move_base', MoveBaseAction,
goal=nav_goal, result_cb=self.move_base_result_cb)

注意我們如何使用關鍵字result_cb分配函數來處理結果。我們的move_base_result_cb函數然後看起來像這樣:

def move_base_result_cb(self, userdata, status, result):
  if status == actionlib.GoalStatus.SUCCEEDED:
    self.n_succeeded += 1 
  elif status == actionlib.GoalStatus.ABORTED:
    self.n_aborted += 1 
  elif status == actionlib.GoalStatus.PREEMPTED:
    self.n_preempted += 1
  try: 
    rospy.loginfo("Success rate: " + str(100.0 * self.n_succeeded / (self.n_succeeded + self.n_aborted + self.n_preempted))) 
  except:
    pass

回調函數接受三個參數：當前用戶數據(稍後我們將探索更多)，以及返回的狀態和潛在的ROS行爲結果(在這種情況下是move_base)。事實證明，move_base動作不使用result字段。相反，它將放置結果到status字段，這就是爲什麼我們的測試條件檢查字段是status值。

可以從上面的代碼中看到，我們的回調是簡單地增加move_base嘗試成功(succeed)、崩潰或搶佔(preempted)的次數。到目前爲止我們還打印出成功(succeed)百分比。

【ROS書籍】ROSByExampleⅡ——第三章使用ROS任務執行 (二)

3.6 腳本的問題

3.7 SMACH或行爲樹？

3.8 SMACH：任務作爲狀態機

3.8.1 SMACH回顧

3.8.2 使用SMACH巡邏一個正方形

3.8.3 在ArbotiX模擬器中測試SMACH導航

3.8.4 從SimpleActionState訪問結果

詐騙（殺豬盤）網站進行滲透測試

Python 潮流週刊#50：我最喜歡的 Python 3.13 新特性！

【Python】保存gym截圖

【譯】使用 GitHub Copilot 作爲你的編碼 GPS

Linux 服務器配置-安裝portainer-ce社區版

外行也能讀懂的網絡硬件設備功能原理速成

安裝Auto-GPT

【ROS總結】Turtlebot ROS 開機自啓動設置

[CoffeeBot] 運行最終腳本

【ROS總結】ROS下的百度語音識別應用

[CoffeeBot] 配置CoffeeBot的網頁服務器

[CoffeeBot] 安裝Chrome瀏覽器擴展

https://yachay.unat.edu.pe/blog/index.php?comment_area=format_blog&comment_component=blog&comment_co

linux以太網驅動總結

【ROS書籍】ROSByExampleⅡ——第三章 使用ROS任務執行 (二)

3.6 腳本的問題

3.7 SMACH或行爲樹？

3.8 SMACH：任務作爲狀態機

3.8.1 SMACH回顧

3.8.2 使用SMACH巡邏一個正方形

3.8.3 在ArbotiX模擬器中測試SMACH導航

3.8.4 從SimpleActionState訪問結果

【ROS書籍】ROSByExampleⅡ——第三章使用ROS任務執行 (二)