(!) Please ask about problems and questions regarding this tutorial on answers.ros.org. Don't forget to include in your question the link to this page, the versions of your OS & ROS, and also add appropriate tags.

开始使用smach

Description: 这个教程是指导你使用smach的第一步。

Tutorial Level: BEGINNER

Next Tutorial: 在状态间传递用户数据

为什么要学习Smach?

Smach代表"状态机",它是一种基于python的强大的、可伸缩的分级状态机库。Smach库不依赖于ROS,并且可以在任何Python项目中使用。executive_smach堆栈提供了与ROS非常好的集成,包括平滑actionlib集成和强大的Smach查看器来可视化和内部化状态机。

编写一个简单的Smach状态机非常简单,同时,Smach允许你设计、维护和调试大型的、复杂的层次结构状态机。下面的图像显示了一个用于协调actionlib操作的示例状态机,它允许PR2机器人在一个标准的电源插座上充电

pr2_plugs_executive/smach.png

创建一个状态机

创建一个Smach状态机之前,你首先需要创建一组状态机,然后添加这些状态机到状态机容器。

创建一个状态

创建一个状态,你只需从"State"基类继承,并实现"State.execute(userdata)"方法:

   1   class Foo(smach.State):
   2      def __init__(self, outcomes=['outcome1', 'outcome2']):
   3        # 你的状态在这里进行初始化
   4 
   5      def execute(self, userdata):
   6         # 你的状态在这里运行
   7         if xxxx:
   8             return 'outcome1'
   9         else:
  10             return 'outcome2'
  • init方法中,你初始化你的状态类。确保在init方法中不要阻塞!如果你需要等待系统的其他部分启动,请从单独的线程执行此操作。

  • 在状态的execute中,实际的状态已经完成。在这里你可以运行任何你想要的代码。只要你愿意,可以在此方法中添加阻塞。一旦你从这个方法返回,当前状态就完成了。

  • 当一个状态完成后,它返回一个outcome(结果)。每个状态都有许多与之相关的可能结果。结果是一个用户定义的字符串,它描述了状态如何结束。一组可能的结果可以是['succeeded(成功)'、'failed(失败)'、'awesome']。过渡到下一个状态将根据前一个状态的结果来指定。

向状态机添加状态

状态机是容纳多个状态的容器。当向状态机容器添加状态时,需要指定状态之间的转换。

   1   sm = smach.StateMachine(outcomes=['outcome4','outcome5'])
   2   with sm:
   3      smach.StateMachine.add('FOO', Foo(),
   4                             transitions={'outcome1':'BAR',
   5                                          'outcome2':'outcome4'})
   6      smach.StateMachine.add('BAR', Bar(),
   7                             transitions={'outcome2':'FOO'})

状态机的结果将如下所示:

simple.png

  • 红色框显示了状态机容器的可能的outcomes(结果),如第1行中所指定的:coutcom4和outcom5。

  • 在第3-5行中,我们将第一个状态添加到容器中,并将其命名为FOO。该公约指出所有状态都应为大写字母命名。如果状态FOO的结果是'outcome1',那么我们将转换到状态BAR。如果状态FOO的结果是'outcome2',那么整个状态机将以'outcome4'为结果退出。
  • 每个状态机容器也是一个状态。因此,你可以通过将状态机容器添加到另一个状态机容器中来搭建状态机

示例

这是一个完整的可运行示例,可以在executive_smach_tutorials包中找到。

https://raw.githubusercontent.com/rhaschke/executive_smach_tutorials/indigo-devel/examples/state_machine_simple.py

   1 #!/usr/bin/env python
   2 
   3 import rospy
   4 import smach
   5 
   6 # define state Foo
   7 class Foo(smach.State):
   8     def __init__(self):
   9         smach.State.__init__(self, outcomes=['outcome1','outcome2'])
  10         self.counter = 0
  11 
  12     def execute(self, userdata):
  13         rospy.loginfo('Executing state FOO')
  14         if self.counter < 3:
  15             self.counter += 1
  16             return 'outcome1'
  17         else:
  18             return 'outcome2'
  19 
  20 
  21 # define state Bar
  22 class Bar(smach.State):
  23     def __init__(self):
  24         smach.State.__init__(self, outcomes=['outcome2'])
  25 
  26     def execute(self, userdata):
  27         rospy.loginfo('Executing state BAR')
  28         return 'outcome2'
  29         
  30 
  31 
  32 
  33 # main
  34 def main():
  35     rospy.init_node('smach_example_state_machine')
  36 
  37     # Create a SMACH state machine
  38     sm = smach.StateMachine(outcomes=['outcome4', 'outcome5'])
  39 
  40     # Open the container
  41     with sm:
  42         # Add states to the container
  43         smach.StateMachine.add('FOO', Foo(), 
  44                                transitions={'outcome1':'BAR', 
  45                                             'outcome2':'outcome4'})
  46         smach.StateMachine.add('BAR', Bar(), 
  47                                transitions={'outcome2':'FOO'})
  48 
  49     # Execute SMACH plan
  50     outcome = sm.execute()
  51 
  52 
  53 if __name__ == '__main__':
  54     main()

运行示例:

对于ROS Kinetic(及以上)版本仅克隆git存储库, cd到"executive_smach_tutorials"目录, 然后运行示例(在你的ROS环境中)

$ git clone https://github.com/eacousineau/executive_smach_tutorials.git
$ cd executive_smach_tutorials
$ ./examples/state_machine_simple.py

对于老的ROS版本,你可以使用rosdep来安装smach_tutorials包。

$ roscd smach_tutorials
$ ./examples/state_machine_simple.py

你将会看到如下输出:

[INFO] 1279835117.234563: Executing state FOO
[INFO] 1279835117.234849: State machine transitioning 'FOO':'outcome1'-->'BAR'
[INFO] 1279835117.235114: Executing state BAR
[INFO] 1279835117.235360: State machine transitioning 'BAR':'outcome2'-->'FOO'
[INFO] 1279835117.235633: Executing state FOO
[INFO] 1279835117.235884: State machine transitioning 'FOO':'outcome1'-->'BAR'
[INFO] 1279835117.236143: Executing state BAR
[INFO] 1279835117.236387: State machine transitioning 'BAR':'outcome2'-->'FOO'
[INFO] 1279835117.236644: Executing state FOO
[INFO] 1279835117.236891: State machine transitioning 'FOO':'outcome1'-->'BAR'
[INFO] 1279835117.237149: Executing state BAR
[INFO] 1279835117.237394: State machine transitioning 'BAR':'outcome2'-->'FOO'
[INFO] 1279835117.237652: Executing state FOO
[INFO] 1279835117.237898: State machine terminating 'FOO':'outcome2':'outcome4'

预定义状态和容器

状态库

上面的示例说明了如何实现自己的状态。然而,Smach附带了一个完整的预先实现的状态库,它涵盖了许多常见的应用程序:

  • SimpleActionState: 自动调用actionlib行为.

  • ServiceState: 自动调用ROS服务

  • MonitorState

  • ...

'Smach状态'部分在教程页面中给出了所有可用状态的概述。

容器库

同样,Smach也有一组有用的容器:

  • StateMachine: 生成状态机容器

  • Concurrence: 可以并行运行多个状态的状态机

  • Sequence: 一种状态机,它使执行一组状态的顺序变得容易。'Smach容器'部分在教程页面给出了所有可用容器的概述。

width="350"

下一个教程将教会你如何在不同状态和状态机之间传递用户数据。

Wiki: cn/smach/Tutorials/Getting Started (last edited 2018-09-29 01:15:48 by Playfish)