SMACH

关于抢占

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

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

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

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

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)

在被抢占状态里定义抢占响应（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'

在代码的后面增加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 = 0def 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 preemptrospy.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 Truedef main():rospy.init_node('smach_example_state_machine')# Create the top level SMACH state machinesm_top = smach.StateMachine(outcomes=['outcome6'])# Open the containerwith sm_top:smach.StateMachine.add('BAS', Bas(),transitions={'outcome3':'CON'})# Create the sub SMACH state machinesm_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 containerwith sm_con:# Add states to the containersmach.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 serversis = 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 applicationrospy.spin()sis.stop()if __name__ == '__main__':main()

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

一些Tips

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

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

待补充

SMACH缺点

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

个人的结论

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

ROS SMACH个人学习记录相关推荐

ROS入门WIKI学习记录
20180511 编写订阅器节点代码解释 34 void chatterCallback(const std_msgs::String::ConstPtr& msg) 35 { 36 ROS ...
ROS学习笔记十一：ROS中数据的记录与重放
ROS学习笔记十一:ROS中数据的记录与重放本节主要介绍如何记录一个正在运行的ROS系统中的数据,然后在一个运行的系统中根据记录文件重新产生和记录时类似的运动情况.本例子还是以小海龟例程为例. 记录 ...
ros学习记录：Gazebo加载速度慢，长时间停在“Preparing your world”
前言 ros学习记录笔记,一个小白的自留地,欢迎大佬批评指正~ 一.问题描述 Gazebo加载速度慢,长时间停在"Preparing your world" 二.解决办法 1.下载 ...
Webots+ROS学习记录（4）——六轮全地形移动机器人
Webots+ROS学习记录(4)--六轮全地形移动机器人有了以上经验,可以创造出一个全地形的移动机器人如图1 第一步,创建robot节点,并给robot节点编写相应参数注意,这里的机身不能再使用 ...
ROS学习记录（二）阿克曼转向车运动学模型及在gazebo中搭建仿真环境
前言:该篇是ROS学习记录的第二篇,如果还没关注过之前文章的读者,如有需要可以通过下方传送门去看之前的文章: ROS学习记录(一) Plugin插件这两天关注了古月老师的公众号,看到了其中一篇课程推 ...
ROS学习记录9——urdf文件的创建与使用
零.前言在学习这一节之前,至少得明白如何创建一个工作空间,如何编译工作空间,如何运行工作空间里的文件(把工作空间的环境目录添加到环境里).如果有不会的,请看鄙人的ROS学习记录3. 本节默认工作空间 ...
【ROS学习记录】2021/7/2 Gazebo+rviz仿真（一）
[ROS学习记录]2021/7/2 Gazebo+rviz进行turtlebot2仿真问题记录 ##小插曲今天ubuntu挂载u盘一直读取失败所以做了一些令人窒息的操作,晚上开机就进入了initra ...
ROS学习记录 | 便于回顾
ROS学习记录 | 便于回顾 ROS环境搭建 ROS部分基础知识 ROS环境搭建 step1 创建工作空间目录结构 mkdir catkin_ws cd catkin_ws mkdir src cd ...
基础篇——ROS入门：学习资料、基本介绍、安装配置、文件系统、包创建
简介本篇文章主要介绍ROS入门基础,通过此篇文章,可以学习到安装配置.文件系统使用.ROS包创建和构建相关内容. ROS资源网站 ROS官网:http://wiki.ros.org/,可以这么说,市 ...

ROS SMACH个人学习记录

ROS SMACH学习个人记录