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

3.8.5 SMACH Iterators

在patrol_smach.py腳本，我們在一個while循環將機器執行重複巡邏。現在，我們將展示如何使用SMACH迭代器(Iterator)容器實現相同的結果。新的腳本叫做patrol_smach_iterator.py，可以在rbx2_tasks/nodes目錄中找到。因爲patrol_smach.py腳本的大多數是一樣的，我們只會突出差異。

程序中的關鍵代碼如下:

# Initialize the top level state machine
self.sm = StateMachine(outcomes=['succeeded','aborted','preempted'])
with self.sm:
  # Initialize the iterator
  self.sm_patrol_iterator = Iterator(outcomes = ['succeeded','preempted','aborted'],
                            input_keys = [],
                            it = lambda: range(0, self.n_patrols),
                            output_keys = [],
                            it_label = 'index',
                            exhausted_outcome = 'succeeded')

with self.sm_patrol_iterator:
  # Initialize the patrol state machine
  self.sm_patrol = StateMachine(outcomes =
  ['succeeded','aborted','preempted','continue'])

# 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','preempted':'NAV_STATE_1'})
  StateMachine.add('NAV_STATE_1', nav_states[1],
  transitions={'succeeded':'NAV_STATE_2','aborted':'NAV_STATE_2','preempted':'NAV_STATE_2'})
  StateMachine.add('NAV_STATE_2', nav_states[2],
  transitions={'succeeded':'NAV_STATE_3','aborted':'NAV_STATE_3','preempted':'NAV_STATE_3'})
  StateMachine.add('NAV_STATE_3', nav_states[3],
  transitions={'succeeded':'NAV_STATE_4','aborted':'NAV_STATE_4','preempted':'NAV_STATE_4'})
  StateMachine.add('NAV_STATE_4', nav_states[0],
  transitions={'succeeded':'continue','aborted':'continue','preempted':'continue'})
  # Close the sm_patrol machine and add it to the iterator
  Iterator.set_contained_state('PATROL_STATE', self.sm_patrol,
                               loop_outcomes=['continue'])
# Close the top level state machine
StateMachine.add('PATROL_ITERATOR', self.sm_patrol_iterator, {'succeeded':'succeeded', 'aborted':'aborted'})

現在，讓我們分塊解釋。

# Initialize the top level state machine
self.sm = StateMachine(outcomes=['succeeded','aborted','preempted'])
with self.sm:
  # Initialize the iterator
  self.sm_patrol_iterator = Iterator(outcomes = ['succeeded','preempted','aborted'],
                            input_keys = [],
                            it = lambda: range(0, self.n_patrols),
                            output_keys = [],
                            it_label = 'index',
                            exhausted_outcome = 'succeeded')

在頂部初始化狀態機後，我們構建一個迭代器(Iterator)循環的self.n_patrols時間。迭代器(Iterator)的核心是it參數，該參數將設置迭代對象的列表。在我們的例子中，我們使用Python的lambda函數定義列表創建一個整數列表通過range(0,self.n_patrols)。it_label參數(在我們的例子中設置爲“index”)保持當前關鍵值，因爲它遍歷列表。exhausted_outcome參數設置發出結果，當迭代器(Iterator)已經到了列表的最後。

with self.sm_patrol_iterator:
  # Initialize the patrol state machine
  self.sm_patrol = StateMachine(outcomes =
  ['succeeded','aborted','preempted','continue'])

# 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','preempted':'NAV_STATE_1'})
  StateMachine.add('NAV_STATE_1', nav_states[1],
  transitions={'succeeded':'NAV_STATE_2','aborted':'NAV_STATE_2','preempted':'NAV_STATE_2'})
  StateMachine.add('NAV_STATE_2', nav_states[2],
  transitions={'succeeded':'NAV_STATE_3','aborted':'NAV_STATE_3','preempted':'NAV_STATE_3'})
  StateMachine.add('NAV_STATE_3', nav_states[3],
  transitions={'succeeded':'NAV_STATE_4','aborted':'NAV_STATE_4','preempted':'NAV_STATE_4'})
  StateMachine.add('NAV_STATE_4', nav_states[0],
  transitions={'succeeded':'continue','aborted':'continue','preempted':'continue'})

接下來我們創建巡邏狀態機，比較之前的兩個差異。首先，在”with self.sm_patrol_iterator”中聲明狀態機是阻塞的。第二，我們已經在整體巡邏機和最終狀態：NAV_STATE_4中添加了一個新的結果標記爲“continue”。爲什麼我們這樣做在下面最後一行會解釋。

  # Close the sm_patrol machine and add it to the iterator
  Iterator.set_contained_state('PATROL_STATE', self.sm_patrol,
                               loop_outcomes=['continue'])
# Close the top level state machine
StateMachine.add('PATROL_ITERATOR', self.sm_patrol_iterator, {'succeeded':'succeeded', 'aborted':'aborted'})

上面的第一行中增加了巡邏狀態機的迭代器(Iterator)所包含的狀態和loop_outcomes參數設置爲“continue”。這意味着當包含的狀態機發出“continue”的結果時，迭代器(Iterator)將移動到列表的下一個值。正如你從我們的巡邏狀態機所看到的，NAV_STATE_4映射所有結果到“continue”，所以一旦我們完成NAV_STATE_4，迭代器(Iterator)將開始下一個循環。如果迭代器(Iterator)到達最後的列表，它將通過一個設定的exhausted_outcomes參數終止結果，所以我們構建迭代器(Iterator)時設置爲“succeeded”。

最後一行在整個狀態機中添加迭代器(Iterator)作爲一個狀態。

測試腳本，確保你有在前面的章節中的虛擬TurtleBot啓動並運行，以及有nav_tasks.rviz配置文件的RViz，然後運行腳本迭代器(Iterator)：

$ rosrun rbx2_tasks patrol_smach_iterator.py

結果應該與以前一樣：機器人應該完成兩個廣場巡邏。

3.8.6 在一個轉換中運行命令

虛擬設我們想讓機器人執行一個或多個函數每次從一個狀態轉換到下一個。例如,也許我們想要巡邏機器人掃描每個房間的人如果看到某人,說你好,然後繼續。

這種類型的執行回調函數可以通過使用轉換。我們的演示腳本叫做patrol_smach_callback.py位於目錄rbx2_tasks /節點。大多數的腳本patrol_smach是一樣的.py腳本前面描述的我們只會突出的差異

首先我們設置轉換回調狀態機如下:

self.sm_patrol.register_transition_cb(self.transition_cb, cb_args=[])

register_transition_cb()函數接受兩個參數:我們想要執行的回調函數,回調參數的列表,可以是一個空列表,這裏我們使用。我們的回調函數,self.transition_cb()是這樣的:

def transition_cb(self, userdata, active_states, *cb_args): 
  if self.rand.randint(0, 3) == 0:
    rospy.loginfo("Greetings human!")
    rospy.sleep(1) 
    rospy.loginfo("Is everything OK?")
    rospy.sleep(1) else:
    rospy.loginfo("Nobody here.")

這裏我們只是虛擬裝檢查一個人的存在通過隨機選擇一個數字0和3之間。如果是0,我們發出問候,否則,我們報告,沒有人存在。當然,在真實的應用程序中,你可能包括代碼掃描房間淘洗機器人的攝像頭和使用語音說話人聊天,如果他們發現。

測試腳本,確保你有虛擬TurtleBot啓動並運行,在前面的章節以及RViz nav_tasks。rviz配置文件,然後運行腳本迭代器:

$ rosrun rbx2_tasks patrol_smach_callback.py

不同於之前的腳本,這一個將與ctrl - c運行下去,直到你中止它

3.8.7 使用ROS話題和服務交互

虛擬設我們希望監視機器人的電池水平,如果低於某一閾值,機器人應該暫停或中止這是做什麼,導航到停靠站和充電,然後在它停止的地方繼續前面的任務。首先,我們需要知道如何使用SMACH監控電池的水平。早些時候我們用虛擬電池模擬器介紹來說明這個過程。

SMACH提供了預定義的狀態MonitorState和ServiceState與ROS話題和服務在一個狀態機。我們將使用一個MonitorState跟蹤模擬電池水平和ServiceState模擬充電。之前將電池檢查集成到我們的巡邏機器人狀態機,讓我們看一個簡單的狀態機,監控電池水平,然後發出一個充電服務調用時水平低於閾值。

被稱爲monitor_fake_battery演示腳本.py腳本rbx2_tasks /節點目錄和看起來像下面的

連接到源代碼：monitor_fake_battery.py

001  #!/usr/bin/env python
002  
003  import rospy
004  from smach import State, StateMachine
005  from smach_ros import MonitorState, ServiceState, IntrospectionServer
006  from rbx2_msgs.srv import *
007  from std_msgs.msg import Float32
008  
009  class main():
010      def __init__(self):
011          rospy.init_node('monitor_fake_battery', anonymous=False)
012          
013          rospy.on_shutdown(self.shutdown)
014          
015          # Set the low battery threshold (between 0 and 100)
016          self.low_battery_threshold = rospy.get_param('~low_battery_threshold', 50)
017   
018          # Initialize the state machine
019          sm_battery_monitor = StateMachine(outcomes=[])
020  
021          with sm_battery_monitor:
022              # Add a MonitorState to subscribe to the battery level topic
023              StateMachine.add('MONITOR_BATTERY',
024                   MonitorState('battery_level', Float32, self.battery_cb), 
025                   transitions={'invalid':'RECHARGE_BATTERY',
026                                'valid':'MONITOR_BATTERY',
027                                'preempted':'MONITOR_BATTERY'},)
028  
029              # Add a ServiceState to simulate a recharge using the set_battery_level service
030              StateMachine.add('RECHARGE_BATTERY',
031                   ServiceState('battery_simulator/set_battery_level', SetBatteryLevel, request=100), 
032                   transitions={'succeeded':'MONITOR_BATTERY',
033                                'aborted':'MONITOR_BATTERY',
034                                'preempted':'MONITOR_BATTERY'})
035              
036          # Create and start the SMACH introspection server
037          intro_server = IntrospectionServer('monitor_battery', sm_battery_monitor, '/SM_ROOT')
038          intro_server.start()
039          
040          # Execute the state machine
041          sm_outcome = sm_battery_monitor.execute()
042                          
043          intro_server.stop()
044          
045      def battery_cb(self, userdata, msg):
046          rospy.loginfo("Battery Level: " + str(msg))
047          if msg.data < self.low_battery_threshold:
048              return False
049          else:
050              return True
051  
052      def shutdown(self):
053          rospy.loginfo("Stopping the battery monitor...")
054          rospy.sleep(1)
055  
056  if __name__ == '__main__':
057      try:
058          main()
059      except rospy.ROSInterruptException:
060        rospy.loginfo("Battery monitor finished.")

讓我們分塊解釋代碼。

004  from smach import State, StateMachine
005  from smach_ros import MonitorState, ServiceState, IntrospectionServer
006  from rbx2_msgs.srv import *
007  from std_msgs.msg import Float32

除了通常的狀態和StateMachine對象，我們也從smach_ros導入MonitorState和ServiceState。由於服務，我們將連接rbx2_msgs包中的(set_battery_level)，將導入所有服務定義。最後，由於電池水平使用Float32消息類型發佈，我們還需從ROS的std_msgs包導入這個類型。

016          self.low_battery_threshold = rospy.get_param('~low_battery_threshold', 50)

low_battery_threshold作爲一個參數從ROS服務器中讀取，默認虛擬設爲50以下，100是完全充電。

019          sm_battery_monitor = StateMachine(outcomes=[])

我們創建一個名爲sm_battery_monitor的頂級狀態機並將其分配給一個空的結果，因爲它不會自己產生結果。

021          with sm_battery_monitor:
022              # Add a MonitorState to subscribe to the battery level topic
023              StateMachine.add('MONITOR_BATTERY',
024                   MonitorState('battery_level', Float32, self.battery_cb), 
025                   transitions={'invalid':'RECHARGE_BATTERY',
026                                'valid':'MONITOR_BATTERY',
027                                'preempted':'MONITOR_BATTERY'},)

我們增加狀態機的第一個狀態稱爲MONITOR_BATTERY，它使用SMACH MonitorState監視電池水平。MonitorState構造函數的參數是這個我們想監控的話題，該話題的消息類型和一個回調函數(這裏稱爲self.battery_cb)描述如下。字典轉換的關鍵名稱來自預定義的結果，對於MonitorState類型是avalid、invalid和preempted，雖然avalid和invalid的結果實際上分別代表值True和False。在我們的例子中，我們的回調函數將使用invalid結果，這意味着電池水平低於閾值，所以我們映射這個鍵到一個轉換到RECHARGE_BATTERY狀態，如以下描述。

030              StateMachine.add('RECHARGE_BATTERY',
031                   ServiceState('battery_simulator/set_battery_level', SetBatteryLevel, request=100), 
032                   transitions={'succeeded':'MONITOR_BATTERY',
033                                'aborted':'MONITOR_BATTERY',
034                                'preempted':'MONITOR_BATTERY'})

我們添加到狀態機的第二個狀態是RECHARGE_BATTERY狀態，該狀態使用SMACH ServiceState。ServiceState構造函數的參數是服務名稱、服務類型和發送到服務請求的值。SetBatteryLevel服務類型在rbx2_msgs包中設置，這就是爲什麼我們在腳本的頂部導入rbx2_msgs.srv。設置請求值爲100基本上是模擬電池的充電過程。ServiceState返回傳統的成功(succeed)、崩潰(aborted)和搶佔(preempted)結果。我們將所有的結果映射回MONITOR_BATTERY狀態。

腳本的最後一部分需要解釋MonitorState的回調函數：

045      def battery_cb(self, userdata, msg):
046          rospy.loginfo("Battery Level: " + str(msg))
047          if msg.data < self.low_battery_threshold:
048              return False
049          else:
050              return True

分配的任何回調函數給MonitorState狀態會自動接收發布的消息到訂閱的話題，和該狀態的userdata。在這個回調，我們只會使用話題的類型(傳入的msg參數)，而不是userdata參數。

回想一下我們監測的消息是簡單Float32數字，該數字代表的是模擬電池水平。上面的第一行battery_cb功能只顯示到屏幕上。然後我們在腳本早些時候測試水平low_battery_threshold。如果當前水平低於low_battery_threshold，我們返回False，相當於到MonitorState時invalid。否則，我們返回True和valid的結果是一樣的。正如我們前面看到的，當MONITOR_BATTERY狀態通過回調函數生成一個invalid結果，它轉換到RECHARGE_BATTERY狀態。

既然我們瞭解腳本的功能，讓我們試試。首先確保虛擬電池運行。如果你運行之前小節中的fake_turtlebot.launch，本小節還運行。否則，你可以啓動自己的虛擬電池：

$ roslaunch rbx2_utils battery_simulator.launch

然後啓動monitor_fake_battery.py腳本：

$ rosrun rbx2_tasks monitor_fake_battery.py

你應該會看到一系列的消息類似如下：

[INFO] [WallTime:1379002809.494314] State machine starting in initial state
'MONITOR_BATTERY' with userdata:[]
[INFO] [WallTime: 1379002812.213581] Battery Level: data: 70.0
[INFO] [WallTime: 1379002813.213005] Battery Level: data: 66.6666641235
[INFO] [WallTime: 1379002814.213802] Battery Level: data: 63.3333320618
[INFO] [WallTime: 1379002815.213758] Battery Level: data: 60.0
[INFO] [WallTime: 1379002816.213793] Battery Level: data: 56.6666679382
[INFO] [WallTime: 1379002817.213799] Battery Level: data: 53.3333320618
[INFO] [WallTime: 1379002818.213819] Battery Level: data: 50.0
[INFO] [WallTime: 1379002819.213816] Battery Level: data: 46.6666679382
[INFO] [WallTime: 1379002819.219875] State machine transitioning
'MONITOR_BATTERY':'invalid'-->'RECHARGE_BATTERY'
[INFO] [WallTime: 1379002819.229251] State machine transitioning
'RECHARGE_BATTERY':'succeeded'-->'MONITOR_BATTERY'
[INFO] [WallTime: 1379002820.213889] Battery Level: data: 100.0
[INFO] [WallTime: 1379002821.213805] Battery Level: data: 96.6666641235
[INFO] [WallTime: 1379002822.213807] Battery Level: data: 93.3333358765 etc

上面的第一個INFO消息表明MONITOR_STATE初始化狀態機的狀態。接下來的一系列行顯示了一個倒計時的電池水平，這是我們在battery_cb函數描述rospy.loginfo()語句的結果。水平低於閾值時，我們看到了MONITOR_STATE返回一個invalid的結果，該結果觸發狀態轉換到RECHARGE_BATTERY狀態。回顧到RECHARGE_STATE狀態來調用set_battery_level服務並設置電池回到100滿水平。然後返回一個成功(succeed)的結果並觸發狀態轉換回到MONITOR_STATE。這個過程會繼續下去。

3.8.8 回調函數(Callbacks)和自省(Introspection)

SMACH的優點之一是能夠在任何給定的時間確定機器的狀態和設置回調狀態轉換或終止。例如，如果一個給定的狀態機被搶佔(preempted)，我們想知道在被中斷前過去的狀態。在下一節我們將使用這個特性來確定充電後繼續回到路上巡邏。我們還可以使用自省(Introspection)來改善我們跟蹤成功率：不是簡單地保持記錄到達中轉地點次數，我們也可以記錄路標ID。

你可以從在線SMACH API找到所有可能的回調細節。我們將使用的回調通常是在一個給定的狀態機向每個狀態提供轉換。回調函數本身設置使用register_transition_cb()方法在狀態機對象上。例如，在我們的巡邏狀態機上的patrol_smach.py腳本中註冊一個轉換回調，我們將使用的語法：

self.sm_patrol.register_transition_cb(self.patrol_transition_cb, cb_args=[])

我們將定義函數self.patrol_transition_cb來在每個狀態轉換執行我們想要執行的任何代碼：

def patrol_transition_cb(self, userdata, active_states, *cb_args):
  Do something awesome with userdata, active_states or cb_args

在這裏，我們看到一個轉換回調總是訪問userdata、active_states和任何傳遞的回調參數。特別是變量active_states保持狀態機的當前狀態，和下一節我們將使用這個來確定機器人位置當中斷並充電。

3.8.9並行任務:將電池檢查添加到日常巡邏

現在，我們知道如何廣場巡邏和檢查電池，是時候把這兩個在一起。我們想要一個低電池信號得到高優先級，這樣機器人會停止巡邏並導航到充電樁。一旦充電完成，我們想機器人繼續巡邏，巡邏位置爲中斷的最後一個路標。

SMACH提供Concurrence容器用於一致的運行任務，當條件滿足時，使一個任務搶佔(preempted)其他任務。我們用Concurrence容器設立新的狀態機來保持導航狀態機和電池檢查狀態機。然後我們將設置容器，這樣當電池電量低於閾值時，電池檢查可以搶佔(preempted)導航。

在看代碼之前，讓我們試一試。從早先的章節中終止monitor_battery.py節點，如果仍在運行。(在殺死節點之前可能需要一段時間響應Ctrl-C。)

如果fake_turtlebot.launch文件沒有運行，現在運行。回想一下，這個文件還啓動了虛擬電池模擬器並運行60秒電池：

$ roslaunch rbx2_tasks fake_turtlebot.launch

如果你還沒有運行，使用命令啓動SMACH查看器：

$ rosrun smach_viewer smach_viewer.py

啓動有nav_task配置文件的RViz，如果尚未運行：

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

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

$ rosrun rbx2_tasks patrol_smach_concurrence.py

你應該看到機器人圍繞廣場移動兩次，然後停止。當電池低於閾值(task_setup.py文件默認設置爲50)，機器人將中斷其巡邏併到充電樁充電。一旦充電完成，它將繼續到離開的地方巡邏。

腳本運行時，你還可以查看SMACH查看器的狀態機。圖像看起來應該像下面這樣：

(如果你沒有在SMACH查看器中看到這張圖片，關閉它然後重新啓動。)圖像在屏幕上應該比在這裏重現更清晰。較大的灰色框左邊代表Concurrence容器包含導航狀態機SM_NAV和電池監控狀態機MONITOR_BATTERY訂閱的電池話題。右邊的這個綠色的小盒子代表RECHARGE狀態機和包含NAV_DOCKING_STATION以及RECHARGE_BATTERY狀態。兩者之間的轉換狀態機可以看到更清晰的放大，如下圖：

滅弧箭頭從左邊的紅色recharge結果到右邊的綠色RECHARGE盒，顯示從concurrence容器到RECHARGE狀態機當concurrence提供“recharge”的結果。

現在讓我們來研究一下代碼,使這項工作。以下是總結的步驟:

l 創建一個名爲sm_nav的狀態機，負責移動機器人從一個路標導航到下一個路標
l 創建一個名爲sm_recharge的第二狀態機,移動機器人到充電樁並執行充電
l 創建一個名爲sm_patrol的第三狀態機定義爲一個concurrence容器對sm_nav狀態機與MonitorState電池狀態訂閱主題,可以搶佔sm_nav機而停止sm_recharge狀態機
l 最後,創建一個名爲sm_top的第四狀態機,包括sm_patrolsm_recharge機器以及停止狀態,允許我們一旦我們完成指定數量的循環終止整個巡邏。

狀態機的樹形圖看起來像這樣:

• sm_top
- ◦ sm_patrol
  - ▪ monitor_battery
  - ▪ sm_nav
- ◦ sm_recharge

完整的腳本文件中可以在rbx2_tasks/nodes目錄下找到patrol_smach_concurrence.py。

鏈接到源碼：patrol_smach_concurrence.py

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

3.8.5 SMACH Iterators

3.8.6 在一個轉換中運行命令

3.8.7 使用ROS話題和服務交互

3.8.8 回調函數(Callbacks)和自省(Introspection)

3.8.9並行任務:將電池檢查添加到日常巡邏

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

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.8.5 SMACH Iterators

3.8.6 在一個轉換中運行命令

3.8.7 使用ROS話題和服務交互

3.8.8 回調函數(Callbacks)和自省(Introspection)

3.8.9並行任務:將電池檢查添加到日常巡邏

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