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两个回调函数，具体说明，请参考代码处的文字解释。

　　实现代码如下：

 #!/usr/bin/env python

 import roslib;
 import time
 import rospy
 import smach
 import smach_ros
 from twisted.internet.defer import succeed

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

     def execute(self, userdata):
         time.sleep(4)
         rospy.loginfo('Executing state BAS')
         return 'succeeded'

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

     def execute(self, userdata):
         rospy.loginfo('Executing state FOO')
         time.sleep(1)
         print "counter:%d"%(self.counter)
         if self.counter < 5:
             self.counter += 1
             time.sleep(3)
             return 'succeeded'
         else:
             return 'aborted'

 # define state Bar1
 class Bar1(smach.State):
     def __init__(self):
         smach.State.__init__(self, outcomes=['succeeded','preempted','aborted'])
         self.task_name = 'task_bar1'
     def execute(self, userdata):
         if self.preempt_requested():#如果暂停，则返回暂停状态
             rospy.loginfo("Preempting %s"%(self.task_name))
             self.recall_preempt()#唤醒，终止暂停
             return 'preempted'
         time.sleep(5)
         rospy.loginfo('Executing state BAR1')
         return 'succeeded'

 # define state Bar2
 class Bar2(smach.State):
     def __init__(self):
         smach.State.__init__(self, outcomes=['succeeded','preempted','aborted'])
         self.task_name ='bar2'
     def execute(self, userdata):
         if self.preempt_requested():#如果暂停，则返回暂停状态
             rospy.loginfo("Preempting %s"%(self.task_name))
             self.recall_preempt()#唤醒，终止暂停
             return 'preempted'
         time.sleep(5)
         rospy.loginfo('Executing state BAR2')
         return 'succeeded'

 # define state Bar3
 class Bar3(smach.State):
     def __init__(self):
         smach.State.__init__(self, outcomes=['succeeded','preempted','aborted'])
         self.task_name = 'task_bar3'
     def execute(self, userdata):
         if self.preempt_requested():#如果暂停，则返回暂停状态
             rospy.loginfo("Preempting %s"%(self.task_name))
             self.recall_preempt()#唤醒，终止暂停
             return 'preempted'
         time.sleep(5)
         rospy.loginfo('Executing state BAR3')
         return 'succeeded'

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

     def execute(self, userdata):
         time.sleep(5)
         rospy.loginfo('Executing state BAS')
         return 'succeeded'

 class ConcurrentExample:
     def __init__(self):
         rospy.init_node('smach_example_state_machine')

         self.last_bar_state = None
         # Create the top level SMACH state machine
         self.sm_top = smach.StateMachine(outcomes=['stop'])

         # Open the container
         with self.sm_top:

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

             # Create the sub SMACH state machine
             self.sm_con = smach.Concurrence(outcomes=['succeeded','preempted','aborted'],
                                        default_outcome='aborted',
                                        #outcome_map = {'succeeded':{'FOO':'succeeded'},
                                        #                 'aborted':{'FOO':'aborted'}},
                                        child_termination_cb = self.child_term_cb,
                                        outcome_cb = self.out_cb
                                        )

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

                 self.sm_bar = smach.StateMachine(outcomes=['succeeded','preempted','aborted'])
                 with self.sm_bar:
                     smach.StateMachine.add('BAR1',Bar1(),
                                            transitions={'succeeded':'BAR2','preempted':'preempted'})
                     smach.StateMachine.add('BAR2',Bar2(),
                                            transitions={'succeeded':'BAR3','preempted':'preempted'})
                     smach.StateMachine.add('BAR3',Bar3(),
                                            transitions={'succeeded':'succeeded','preempted':'preempted'})
                 self.sm_bar.register_transition_cb(self.bar_transition_cb, cb_args=[])
                 smach.Concurrence.add('BAR', self.sm_bar)

             smach.StateMachine.add('CON', self.sm_con,
                                    transitions={'succeeded':'stop',
                                                 'aborted':'stop',
                                                 'preempted':'CHARGE'})

             smach.StateMachine.add('CHARGE', Charge(),
                                    transitions={'succeeded':'CON'})

          # Create and start the introspection server
         sis = smach_ros.IntrospectionServer('server_name', self.sm_top, '/SM_ROOT')
         sis.start()

         # Execute SMACH plan
         outcome = self.sm_top.execute()
         rospy.spin()
         sis.stop()

     #状态之间转换的时候会调用该函数。比如BAR1转换到BAR2（或者BAR2转换到BAR3）后，执行该回调函数，
     #那么活动的状态 active_states 是‘BAR2‘（‘BAR3‘）
     def bar_transition_cb(self, userdata, active_states, *cb_args):
         print active_states # 注意这里是字符串，活动状态的标识符例如‘BAR’
         self.last_bar_state = active_states

     # gets called when ANY child state terminates,
     # 只要Concurent下的其中一个状态完成，都会出发该回调函数
     def child_term_cb(self, outcome_map):

       # terminate all running states if FOO preempted with outcome 'succeeded'
       if outcome_map['FOO'] == 'succeeded':
         print "child_term_cv:FOO finished"
         if self.last_bar_state is not None:

             self.sm_bar.set_initial_state(self.last_bar_state, smach.UserData())
         return True

       # terminate all running states if BAR preempted
       if outcome_map['BAR']=='succeeded' or outcome_map['BAR']=='preempted':
         print "child_term_cv:SM_BAR finished"

         return True

       # in all other case, just keep running, don't terminate anything
       return False

     # gets called when ALL child states are terminated，只要Concurrent下的状态都结束了，
     #调用该函数.注意不是BAR下面的BAR1，BAR2，BAR3的之一完成
     def out_cb(self, outcome_map):
        if outcome_map['FOO'] == 'aborted':
           print "out_cb FOO aborted"
           return 'aborted'
        elif outcome_map['BAR'] == 'preempted':

           print "out_cb BAR preempted"
           return 'preempted'
        elif outcome_map['BAR'] == 'succeeded':
            print "out_cb_BAR succeeded"
            return 'succeeded'
 if __name__ == '__main__':
     ConcurrentExample()

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

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

参考资料：

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

[2]. ros_by_example_vol2_indigo.pdf

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