【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