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)百分比。