SMACH专题（二）----Concurrent状态机

Concurrent状态机是一种同时执行多个状态的状态机。如下图所示。状态FOO和BAR同时执行，当两个状态输出的结果同时满足一个组合条件时（FOO输出outcome2,BAR输出outcome1）才会映射到状态CON的输出结果outcome4。

1、简单例子

具体地，实现代码如下：

#!/usr/bin/env python

import roslib; roslib.load_manifest('smach_example')
import time
import rospy
import smach
import smach_ros

# define state Foo
class Foo(smach.State):
    def __init__(self):
        smach.State.__init__(self, outcomes=['outcome1','outcome2'])
        self.counter = 0

    def execute(self, userdata):
        rospy.loginfo('Executing state FOO')
        time.sleep(1)
        if self.counter < 5:
            self.counter += 1
            return 'outcome1'
        else:
            return 'outcome2'


# define state Bar
class Bar(smach.State):
    def __init__(self):
        smach.State.__init__(self, outcomes=['outcome1'])

    def execute(self, userdata):
        time.sleep(1)
        rospy.loginfo('Executing state BAR')
        return 'outcome1'
        


# define state Bas
class Bas(smach.State):
    def __init__(self):
        smach.State.__init__(self, outcomes=['outcome3'])

    def execute(self, userdata):
        rospy.loginfo('Executing state BAS')
        return 'outcome3'




def main():
    rospy.init_node('smach_example_state_machine')

    # Create the top level SMACH state machine
    sm_top = smach.StateMachine(outcomes=['outcome6'])
    
    # Open the container
    with sm_top:

        smach.StateMachine.add('BAS', Bas(),
                               transitions={'outcome3':'CON'})

        # Create the sub SMACH state machine
        sm_con = smach.Concurrence(outcomes=['outcome4','outcome5'],
                                   default_outcome='outcome4',
                                   outcome_map={'outcome5':
                                       { 'FOO':'outcome2',
                                         'BAR':'outcome1'}})

        # Open the container
        with sm_con:
            # Add states to the container
            smach.Concurrence.add('FOO', Foo())
            smach.Concurrence.add('BAR', Bar())

        smach.StateMachine.add('CON', sm_con,
                               transitions={'outcome4':'CON',
                                            'outcome5':'outcome6'})
        
     # Create and start the introspection server
    sis = smach_ros.IntrospectionServer('server_name', sm_top, '/SM_ROOT')
    sis.start()

    # Execute SMACH plan
    outcome = sm_top.execute()
    rospy.spin()
    sis.stop()
if __name__ == '__main__':
    main()

2、复杂例子

　　定义复杂的输出结果条件判断，需要实现两个回调函数，如下代码的156行child_term_cb，178行的out_cb两个回调函数，具体说明，请参考代码处的文字解释。

　　实现代码如下：

  1 #!/usr/bin/env python
  2 
  3 import roslib;
  4 import time
  5 import rospy
  6 import smach
  7 import smach_ros
  8 from twisted.internet.defer import succeed
  9 
 10 
 11 # define state Bas
 12 class Bas(smach.State):
 13     def __init__(self):
 14         smach.State.__init__(self, outcomes=['succeeded'])
 15 
 16     def execute(self, userdata):
 17         time.sleep(4)
 18         rospy.loginfo('Executing state BAS')
 19         return 'succeeded'
 20     
 21 # define state Foo
 22 class Foo(smach.State):
 23     def __init__(self):
 24         smach.State.__init__(self, outcomes=['succeeded','preempted','aborted'])
 25         self.counter = 0
 26 
 27     def execute(self, userdata):
 28         rospy.loginfo('Executing state FOO')
 29         time.sleep(1)
 30         print "counter:%d"%(self.counter)
 31         if self.counter < 5:
 32             self.counter += 1
 33             time.sleep(3)
 34             return 'succeeded'
 35         else:
 36             return 'aborted'
 37 
 38 
 39 # define state Bar1
 40 class Bar1(smach.State):
 41     def __init__(self):
 42         smach.State.__init__(self, outcomes=['succeeded','preempted','aborted'])
 43         self.task_name = 'task_bar1'
 44     def execute(self, userdata):
 45         if self.preempt_requested():#如果暂停，则返回暂停状态
 46             rospy.loginfo("Preempting %s"%(self.task_name))
 47             self.recall_preempt()#唤醒，终止暂停
 48             return 'preempted'
 49         time.sleep(5)
 50         rospy.loginfo('Executing state BAR1')
 51         return 'succeeded'
 52     
 53 # define state Bar2
 54 class Bar2(smach.State):
 55     def __init__(self):
 56         smach.State.__init__(self, outcomes=['succeeded','preempted','aborted'])
 57         self.task_name ='bar2'
 58     def execute(self, userdata):
 59         if self.preempt_requested():#如果暂停，则返回暂停状态
 60             rospy.loginfo("Preempting %s"%(self.task_name))
 61             self.recall_preempt()#唤醒，终止暂停
 62             return 'preempted'
 63         time.sleep(5)
 64         rospy.loginfo('Executing state BAR2')
 65         return 'succeeded'
 66     
 67     
 68 # define state Bar3
 69 class Bar3(smach.State):
 70     def __init__(self):
 71         smach.State.__init__(self, outcomes=['succeeded','preempted','aborted'])
 72         self.task_name = 'task_bar3'
 73     def execute(self, userdata):
 74         if self.preempt_requested():#如果暂停，则返回暂停状态
 75             rospy.loginfo("Preempting %s"%(self.task_name))
 76             self.recall_preempt()#唤醒，终止暂停
 77             return 'preempted'
 78         time.sleep(5)
 79         rospy.loginfo('Executing state BAR3')
 80         return 'succeeded'
 81  
 82 # define state Charge
 83 class Charge(smach.State):
 84     def __init__(self):
 85         smach.State.__init__(self, outcomes=['succeeded'])
 86 
 87     def execute(self, userdata):
 88         time.sleep(5)
 89         rospy.loginfo('Executing state BAS')
 90         return 'succeeded'
 91 
 92 class ConcurrentExample:
 93     def __init__(self):
 94         rospy.init_node('smach_example_state_machine')
 95     
 96         self.last_bar_state = None
 97         # Create the top level SMACH state machine
 98         self.sm_top = smach.StateMachine(outcomes=['stop'])
 99         
100         # Open the container
101         with self.sm_top:
102     
103             smach.StateMachine.add('BAS', Bas(),
104                                    transitions={'succeeded':'CON'})
105     
106             # Create the sub SMACH state machine
107             self.sm_con = smach.Concurrence(outcomes=['succeeded','preempted','aborted'],
108                                        default_outcome='aborted',
109                                        #outcome_map = {'succeeded':{'FOO':'succeeded'},
110                                        #                 'aborted':{'FOO':'aborted'}},
111                                        child_termination_cb = self.child_term_cb,
112                                        outcome_cb = self.out_cb
113                                        )
114     
115             # Open the container
116             with self.sm_con:
117                 # Add states to the container
118                 smach.Concurrence.add('FOO', Foo())
119                 
120                 self.sm_bar = smach.StateMachine(outcomes=['succeeded','preempted','aborted'])
121                 with self.sm_bar:
122                     smach.StateMachine.add('BAR1',Bar1(),
123                                            transitions={'succeeded':'BAR2','preempted':'preempted'})
124                     smach.StateMachine.add('BAR2',Bar2(),
125                                            transitions={'succeeded':'BAR3','preempted':'preempted'})
126                     smach.StateMachine.add('BAR3',Bar3(),
127                                            transitions={'succeeded':'succeeded','preempted':'preempted'})
128                 self.sm_bar.register_transition_cb(self.bar_transition_cb, cb_args=[])
129                 smach.Concurrence.add('BAR', self.sm_bar)
130     
131             smach.StateMachine.add('CON', self.sm_con,
132                                    transitions={'succeeded':'stop',
133                                                 'aborted':'stop',
134                                                 'preempted':'CHARGE'})
135             
136             smach.StateMachine.add('CHARGE', Charge(),
137                                    transitions={'succeeded':'CON'})
138             
139          # Create and start the introspection server
140         sis = smach_ros.IntrospectionServer('server_name', self.sm_top, '/SM_ROOT')
141         sis.start()
142     
143         # Execute SMACH plan
144         outcome = self.sm_top.execute()
145         rospy.spin()
146         sis.stop()
147     
148     #状态之间转换的时候会调用该函数。比如BAR1转换到BAR2（或者BAR2转换到BAR3）后，执行该回调函数，
149     #那么活动的状态 active_states 是‘BAR2‘（‘BAR3‘）
150     def bar_transition_cb(self, userdata, active_states, *cb_args):    
151         print active_states # 注意这里是字符串，活动状态的标识符例如‘BAR’
152         self.last_bar_state = active_states
153         
154     # gets called when ANY child state terminates,
155     # 只要Concurent下的其中一个状态完成，都会出发该回调函数
156     def child_term_cb(self, outcome_map):
157       
158       # terminate all running states if FOO preempted with outcome 'succeeded'
159       if outcome_map['FOO'] == 'succeeded':
160         print "child_term_cv:FOO finished"
161         if self.last_bar_state is not None:
162             
163             self.sm_bar.set_initial_state(self.last_bar_state, smach.UserData())     
164         return True
165           
166       # terminate all running states if BAR preempted
167       if outcome_map['BAR']=='succeeded' or outcome_map['BAR']=='preempted':
168         print "child_term_cv:SM_BAR finished"
169         
170         return True
171     
172       # in all other case, just keep running, don't terminate anything
173       return False
174     
175     
176     # gets called when ALL child states are terminated，只要Concurrent下的状态都结束了，
177     #调用该函数.注意不是BAR下面的BAR1，BAR2，BAR3的之一完成
178     def out_cb(self, outcome_map):
179        if outcome_map['FOO'] == 'aborted':
180           print "out_cb FOO aborted"
181           return 'aborted'
182        elif outcome_map['BAR'] == 'preempted':
183           
184           print "out_cb BAR preempted"
185           return 'preempted'
186        elif outcome_map['BAR'] == 'succeeded':
187            print "out_cb_BAR succeeded"
188            return 'succeeded'
189 if __name__ == '__main__':
190     ConcurrentExample()

　　状态机器效果图，CON下的FOO和BAR同时执行，如下所示：

　　　　CON进入暂停状态，切换到CHARGE状态下执行（绿色表示执行）：

参考资料：

[1]. http://wiki.ros.org/smach/Tutorials/Concurrent%20States

[2]. ros_by_example_vol2_indigo.pdf

问题：只要BAR或FOO之一结束，就输出相应打结果，这个如何做到？还没找到方法