ROS SMACH个人学习记录

本文仅为个人学习记录，结论正确性待考究。欢迎大家讨论

SMACH

关于抢占

抢占需要在并发容器里面实现，并发容器里面包含多个状态，我们分成两类：抢占状态与被抢占状态
抢占的实现原理：

1. 定义子状态结束回调函数，该函数在并发容器里面的任何状态结束时候都会调用

def child_cb(outcome_map):
    rospy.loginfo('excute child call back')
    return True

2. 在并发容器里面增加子状态结束回调函数调用（看最后一行）

sm_con = smach.Concurrence(outcomes=['outcome4','preempted'],
                                   default_outcome='outcome4',
                                   outcome_map={'preempted':{ 'FOO':'outcome1','BAR':'preempted'},
                                                'outcome4':{'BAR':'outcome2'}},
                                    child_termination_cb = child_cb)

3. 在被抢占状态里定义抢占响应（excute后面四行）

# define state Bar
class Bar(smach.State):
    def __init__(self):
        smach.State.__init__(self, outcomes=['outcome2','preempted'])
    def execute(self, userdata):
        rospy.loginfo('Executing state BAR')
        if self.preempt_requested():
            self.service_preempt()
            return 'preempted'
        rospy.sleep(50)
        return 'outcome2'

4. 在代码的后面增加handlerset_preempt_handler(你的状态机名)

全部代码：

#!/usr/bin/env python3

# 在并发状态机里面测试状态抢占功能

import rospy
import smach
import smach_ros
from smach_ros import ServiceState, SimpleActionState, IntrospectionServer,set_preempt_handler, MonitorState


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

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



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

    def execute(self, userdata):
        rospy.loginfo('Executing state BAR')
        if self.preempt_requested():
            self.service_preempt()
            return 'preempted'
        # n=1
        # while n<50:
        #     rospy.sleep(1)
        #     rospy.loginfo('Do something')
        #     n+=1
        # Check for preempt
        rospy.sleep(50)
        return 'outcome2'
        


# 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 child_cb(outcome_map):
    rospy.loginfo('excute child call back')
    return True


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','preempted'],
                                   default_outcome='outcome4',
                                   outcome_map={'preempted':{ 'FOO':'outcome1','BAR':'preempted'},
                                                'outcome4':{'BAR':'outcome2'}},
                                    child_termination_cb = child_cb)

        # 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',
                                            'preempted':'outcome6'})
    # Create and start the introspection server
    sis = smach_ros.IntrospectionServer('server_name', sm_top, '/SM_ROOT')
    sis.start()

    # sm_top.request_preempt()
    set_preempt_handler(sm_top)
    outcome = sm_top.execute()

    # Wait for ctrl-c to stop the application
    rospy.spin()
    sis.stop()

if __name__ == '__main__':
    main()

该例子跑出来的效果：
能够完成Foo对Bar的抢占，不抢占的话会反复在CON和Bar状态里循环，但是Foo要等Bar执行完毕才去抢占。

一些Tips

1. 状态切换太快node会死掉
2. 各个容器可以不定义结果，但建议最好要定义结果（这里的结果表示为在SMACH viewer里面的红色块），子容器与父容器通过该结果联系。子容器可以结果通向父容器或者重新回到自己状态的开始，父容器不能通向到子容器里面（待验证）
3. 子容器与父容器的连线结果等于子容器的红色块结果
4. 并发容器里面嵌套状态机时，如果里面的状态机里面状态阻塞为得到红色快结果，Ctrl C会报错：Concurrent state ‘xxx’ returned no outcome on termination.
5. 抢占在并发容器里面实现，状态1要抢占状态2必须要等到状态2执行完？？？（无action的情况下）
6. Monitor提供消息与状态的交互（monitor call back return true时状态输出invalid，并结束monitor状态，否则monitor状态一直阻塞）
7. 并发容器下只能并行执行多个单状态，如果需要在其中一个状态下再增加状态，需要打包。即对并发容器呈现出来只有多个单状态的并行。举个例子：

这样是不行的
必须把状态2和状态3打包成一个整体，打包在sm_sub1里面

8. 待补充

SMACH缺点

1. 基于Tips7带来的问题：并发容器里的子状态是嵌套的状态机时，如何定义抢占响应。对于各个状态我们可以在里面定义def excute，状态机咋搞？
2. 基于Tips5似乎即便没有缺点1，该抢占也不是真正意义上的抢占？？？
3. SMACH viewer node 经常动不动崩溃

个人的结论

层级数目超过教程例子的状态机就不建议参考使用SMACH，在确定要使用SMACH的情况下，建议在状态机设计时尽可能贴近SMACH教程的架构。