ROS中的samch状态机编写一文搞定

白云千载尽

已于 2024-12-29 15:41:05 修改

阅读量1.1k

点赞数 16

CC 4.0 BY-SA版权

文章标签： python 开发语言 ubuntu ros linux smach

于 2024-12-27 00:45:07 首次发布

本文链接：https://blog.youkuaiyun.com/hooksten/article/details/144750654

这里写目录标题

普通状态机
并行状态机
嵌套状态机
服务状态机
回调状态机
状态机中传递数据
顺序执行状态机
状态中断与返回机制
动作状态机
状态机封装

普通状态机

1.smach.StateMachine()

最为简单的一种smach，只需要定义好输入输出，然后往里面添加状态就行了，特点是简单高效，容易管理。可以通过rosros samch_viewer smach_viewer来可视化。这里只说一个需要注意的地方，那就是我们的init方法是声明这个变量的，然后真正的初始化最好放到execute函数中，不然再重复利用这个类的时候是不会对我们的参数进行初始化的。

在这里插入图片描述

class Foo(smach.State):
    def __init__(self):
        smach.State.__init__(self, outcomes=["outcome1", "outcome2"])
        self.counter = None
        
    def execute(self, userdata):
	    self.counter = 0
        rospy.loginfo('Executin state FOO')
        time.sleep(5)
        if self.counter < 3:
            self.counter += 1
            return 'outcome1'
        
        else:
            return 'outcome2'
        
class Bar(smach.State):
    def __init__(self):
        smach.State.__init__(self, outcomes=['outcome2'])
        
    def execute(self, userdata):
        rospy.loginfo("Executin state BAR")
        time.sleep(5)
        return 'outcome2'

class Bas(smach.State):
    def __init__(self):
        smach.State.__init__(self, outcomes=['finish'])
        
    def execute(self, userdata):
        rospy.loginfo("Executin state BAS")
        time.sleep(5)
        return 'finish'
    
    
def main():
    rospy.init_node('smach_example_state_machine')
    sm = smach.StateMachine(outcomes=['outcome4', 'outcome5'])
    
    with sm:
        smach.StateMachine.add('FOO', Foo(), transitions={'outcome1':'BAR',
                                                          'outcome2':'BAS'})
        
        smach.StateMachine.add('BAR', Bar(), transitions={'outcome2':'FOO'})
        
        smach.StateMachine.add('BAS', Bas(), transitions={'finish':'outcome5'})
        
    # visualize 
    sis = smach_ros.IntrospectionServer('my_smach_introspection_server', sm, '/SM_ROOT')
    sis.start()
    
    outcome = sm.execute()
    
    rospy.spin()
    sis.stop()
    
if __name__ == '__main__':
    main()

并行状态机

2.smach.Concurrence()

并行状态机，用来同时执行一些过程使用的，在实际过程中非常有用，有些时候状态是可以同时进行的，一种方式是通过thread创建新的线程，第二种就是通过并行状态机来实现

在这里插入图片描述

import rospy
import smach
import smach_ros
import time

class Foo(smach.State):
    def __init__(self):
        smach.State.__init__(self, outcomes=["outcome1", "outcome2"])
        self.counter = None
        self.start_time = None
        
        
    def execute(self, userdata):
        self.counter = 0
        self.start_time = time.time()
        
        rospy.loginfo('Executing state FOO')
        while time.time() - self.start_time < 3:
            print("state foo: ", time.time())
            time.sleep(0.5)
            self.counter += 1
        if self.counter > 3:
            return 'outcome1'
        else:
            return 'outcome2'

class Bar(smach.State):
    def __init__(self):
        smach.State.__init__(self, outcomes=['outcome1', 'outcome2'])
        self.counter = None
        self.start_time = None
                
    def execute(self, userdata):
        rospy.loginfo("Executing state BAR")
        self.counter = 0
        self.start_time = time.time()
        
        # print("bar")
        while time.time() - self.start_time < 5:
            print("state bar: ", time.time())
            time.sleep(0.5)
            self.counter += 1
        
        if self.counter > 3:
            self.counter = 0
            return 'outcome1'
        else:
            return 'outcome2'

class Bas(smach.State):
    def __init__(self):
        smach.State.__init__(self, outcomes=['finish'])
        
    def execute(self, userdata):
        rospy.loginfo("Executing state BAS")
        time.sleep(1)
        return 'finish'

def main():
    rospy.init_node('smach_example_state_machine')
    sm = smach.StateMachine(outcomes=['outcome4', 'outcome5'])
    
    with sm:
        smach.StateMachine.add('FOO', Foo(), transitions={'outcome1':'BAS',
                                                          'outcome2':'BAS'})
        
        # smach.StateMachine.add('BAR', Bar(), transitions={'outcome2':'FOO'})
        
        smach.StateMachine.add('BAS', Bas(), transitions={'finish':'PARALLEL'})
        
        # 添加并行状态
        concurrence = smach.Concurrence(outcomes=['outcome4', 'outcome5'], # 本状态的两种状态输出
                                        default_outcome='outcome5', # 默认输出 一般都是循环执行？？？
                                        outcome_map={'outcome4': {'FOO': 'outcome1', 'BAR': 'outcome1'}}) # 满足条件后的输出
        
        with concurrence:
            smach.Concurrence.add('FOO', Foo())
            smach.Concurrence.add('BAR', Bar())
        
        smach.StateMachine.add('PARALLEL', concurrence, 
                              transitions={'outcome4':'outcome4', 
                                          'outcome5':'outcome5'})

    # visualize
    sis = smach_ros.IntrospectionServer('my_smach_introspection_server', sm, '/SM_ROOT')
    sis.start()
    
    outcome = sm.execute()
    
    rospy.spin()
    sis.stop()
    
if __name__ == '__main__':
    main()

嵌套状态机

3.smach.StateMachine() but nesting

循环状态执行过程中是可以有嵌套的，嵌套之后，我们的很多状态被合成了一个状态，大状态机和里面的状态相对独立，所以可以使用同一个类，并且是同样的状态名来管理。嵌套的关键点在于（其实也是状态机本身的设计上的要注意的问题）：要连接号每一个状态的输入和输出

在这里插入图片描述

import smach
import smach.state
import smach_ros
import rospy
import time

class Foo(smach.State):
    def __init__(self):
        smach.State.__init__(self, outcomes=['outcome1', 'outcome2'], 
                             input_keys=['foo_counter_in'], 
                             output_keys=['foo_counter_out'])
        self.counter = None
        self.start_time = None
        
    def execute(self, userdata):
        self.counter = 0
        self.start_time = rospy.Time.now()
        rospy.loginfo('Executing state FOO')
        while (rospy.Time.now() - self.start_time).to_sec() < 3:
            self.counter += 1
        if self.counter > 3:
            return 'outcome1'
        else:
            return 'outcome2'
        
class Bar(smach.State):
    def __init__(self):
        smach.State.__init__(self, outcomes=['outcome1', 'outcome2'], 
                             input_keys=['bar_counter_in'])
        self.counter = None
        self.start_time = None
        
    def execute(self, userdata):
        self.counter = 0
        self.start_time = rospy.Time.now()
        rospy.loginfo('Executing state BAR')

        while (rospy.Time.now() - self.start_time).to_sec() < 5:
            self.counter += 1
            print("BAR", self.counter)
        if self.counter > 3:
            return 'outcome1'
        else:
            return 'outcome2'
    
class Bas(smach.State):
    def __init__(self):
        smach.State.__init__(self, outcomes=['outcome1', 'outcome2'])
        self.counter = None
        self.start_time = None
        
    def execute(self, userdata):
        self.counter = 0
        self.start_time = rospy.Time.now()
        rospy.loginfo('Executing state BAR')

        while (rospy.Time.now() - self.start_time).to_sec() < 2:
            self.counter += 1
            print("BAS", self.counter)
        if self.counter > 3:
            return 'outcome1'
        else:
            return 'outcome2'
    
def main():
    
    rospy.init_node('smach_userdata')
    
    # 整个状态机的所有的输出都在这里定义
    sm = smach.StateMachine(outcomes=['outcome5', 'outcome4'])
    
    with sm:
        # 状态机的输入是在类中，输出则根据状态机来判定
        smach.StateMachine.add('FOO', Foo(), transitions={'outcome1':'BAR', 
                                                          'outcome2':'outcome4'})
        
        smach.StateMachine.add('BAR', Bar(), transitions={'outcome1':'outcome5',
                                                          'outcome2':'outcome4'})
        
        sm_sub = smach.StateMachine(outcomes=['outcome5', 'outcome4']) # 这里需要定义好外部的返回，使用的是内部的最后的输出
        with sm_sub:
            # 所有的状态的输入则都是在类中进行定义的
            smach.StateMachine.add('FOO', Foo(), transitions={'outcome1':'BAS',
                                                              'outcome2':'outcome4'})
            
            smach.StateMachine.add('BAS', Bas(), transitions={'outcome1':'outcome5',
                                                              'outcome2':'outcome4'})
        
        smach.StateMachine.add('SUB_SM', sm_sub, transitions={'outcome5':'outcome5'})
        
    sis = smach_ros.IntrospectionServer('my_smach_introspection_server', sm, '/SM_ROOT')

    sis.start()
    
    outcome = sm.execute()
    rospy.spin()
    sis.stop()
    
if __name__ == '__main__':
    main()

服务状态机

4.smach & service

服务状态也是一种编写状态机的方式，服务是一对一的，服务端负责创建对应的服务，然后客户端负责传入参数请求服务（request）并接收服务端的反馈（response），所以我们要在状态机中实际使用时，需要将客户端写入，然后把参数传给服务端，让规划控制感知层去处理这个客户端的请求来完成相应的功能。接下来我们先来实现一个服务的实例。

catkin_create_pkg smach_test rospy std_msgs
# 然后创建srv目录新建文件得到AddTwoInts.srv：
# request
int64 a
int64 b
---
int64 sum

# 修改cmakelist，添加:
find_package(catkin REQUIRED COMPONENTS
  rospy
  std_msgs
  message_generation
)

add_service_files(
	FILES
	AddTwoInts.srv
)

catkin_package(
CATKIN_DEPENDS message_runtime
)

# 修改package.xml：
  <build_depend>message_generation</build_depend>

  <exec_depend>message_runtime</exec_depend>

# 服务端测试：
#!/usr/bin/env python3
import rospy
from smach_test.srv import AddTwoInts, AddTwoIntsResponse

def handle_add_two_ints(req):
	print(f"Returning {req.a} {req.b} {req.a + req.b}")
	return AddTwoIntsResponse(req.a + req.b)
	
def add_two_ints_server():
	rospy.init_node('add_two_ints_server')
	s = rospy.Service('add_two_ints', AddTwoInts, handle_add_two_ints)
	print("Ready to add two ints.")
	rospy.spin()
	
if __name__ == "__main__":
	add_two_ints_server()


# 客户端测试：
import rospy
import sys
from smach_test.srv import AddTwoInts

def add_two_ints_client(x, y):
	print("wait")
	rospy.wait_for_service('add_two_ints')
	try:
		print("srv connetct")
		add_two_ints = rospy.ServiceProxy('add_two_ints', AddTwoInts)
		resp1 = add_two_ints(x, y)
		print(f"resp1.sum: {resp1.sum}")
		return resp1.sum
	except rospy.ServiceException as e:
		print(f"service call failed {e}")

	
	
if __name__ == "__main__":
    x = 2
    y = 3
    add_two_ints_client(x, y)

service

client

接下来通过这个服务来实现服务状态机（这个是我最早接触到状态机的时候学到的就是这个服务和动作的状态机，感觉非常有用，于是就定下了基调）。这里借这个机会，我们学习一下状态机中的参数传递，可以看到我们使用sm.userdata.request_result = 0这里传入了初始的参数后，就可以在execute函数中去修改和获取这个参数了。通过output_keys=['request_result']指名输出（会对其修改），以及input_keys=['request_result']

在这里插入图片描述

#!/usr/bin/env python

import rospy
import smach
import smach_ros
from smach_test.srv import AddTwoInts, AddTwoIntsRequest, AddTwoIntsResponse

class RequestService(smach.State):
    def __init__(self):
        smach.State.__init__(self, outcomes=['succeeded', 'aborted'],
                             output_keys=['request_result'])

    def execute(self, userdata):
        rospy.loginfo('Executing state REQUEST_SERVICE')

        rospy.wait_for_service('add_two_ints')
        
        try:
            add_two_ints = rospy.ServiceProxy('add_two_ints', AddTwoInts)
            int_x = 1
            int_y = 2
            response = add_two_ints(int_x, int_y)
            userdata.request_result = response.sum
            return 'succeeded'
        except rospy.ServiceException as e:
            rospy.logerr("Service call failed: %s" % e)
            return 'aborted'

class WaitForResult(smach.State):
    def __init__(self):
        smach.State.__init__(self, outcomes=['succeeded', 'timeout'],
                             input_keys=['request_result'])

    def execute(self, userdata):
        rospy.loginfo('Executing state WAIT_FOR_RESULT')
        rospy.sleep(1.0)
        if userdata.request_result > 0:
            return 'succeeded'
        else:
            return 'timeout'

class EvaluateResult(smach.State):
    def __init__(self):
        smach.State.__init__(self, outcomes=['succeeded', 'retry', 'aborted'],
                             input_keys=['request_result'])

    def execute(self, userdata):
        rospy.loginfo('Executing state EVALUATE_RESULT')
        if userdata.request_result == 5:
            return 'succeeded'
        elif userdata.request_result < 5:
            return 'retry'
        else:
            return 'aborted'

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

    # Create a SMACH state machine
    sm = smach.StateMachine(outcomes=['succeeded', 'aborted'])
    sm.userdata.request_result = 0

    with sm:
        smach.StateMachine.add('REQUEST_SERVICE', RequestService(),
                               transitions={'succeeded': 'WAIT_FOR_RESULT',
                                            'aborted': 'aborted'})
        smach.StateMachine.add('WAIT_FOR_RESULT', WaitForResult(),
                               transitions={'succeeded': 'EVALUATE_RESULT',
                                            'timeout': 'aborted'})
        smach.StateMachine.add('EVALUATE_RESULT', EvaluateResult(),
                               transitions={'succeeded': 'succeeded',
                                            'retry': 'REQUEST_SERVICE',
                                            'aborted': 'aborted'})

    sis = smach_ros.IntrospectionServer('server_name', sm, '/SM_ROOT')
    sis.start()

    outcome = sm.execute()

    rospy.spin()
    sis.stop()

if __name__ == '__main__':
    main()

回调状态机

smach.CBState()

状态机中传递数据

userdata

顺序执行状态机

userdata

状态中断与返回机制

通过userdata来存储返回机制

（但是问题是返回去的时候没有从原位开始好像也不行）所以最终版本的停障需要是一个并行的状态，只有某些情况下才进行并行处理

动作状态机

smach_ros.SimpleActionState()

状态机封装

smach_ros.ActionServerWrapper()

将状态机封装成一个动作服务器

下期代码

#!/usr/bin/env python

import rospy
import smach
import smach_ros
from std_srvs.srv import SetBool, SetBoolResponse

# 定义第一个任务状态
class TaskState1(smach.State):
    def __init__(self):
        smach.State.__init__(self, outcomes=['succeeded', 'preempted'], input_keys=['current_task'], output_keys=['current_task'])

    def execute(self, userdata):
        userdata.current_task = 'TASK1'
        rate = rospy.Rate(1)  # 每秒循环一次
        for i in range(5):  # 模拟任务执行5次
            if self.preempt_requested():
                rospy.loginfo('TaskState1: Preempted by emergency stop service.')
                self.service_preempt()
                return 'preempted'
            
            rospy.loginfo('Executing TaskState1...')
            rate.sleep()

        rospy.loginfo('TaskState1 completed.')
        return 'succeeded'

# 定义第二个任务状态
class TaskState2(smach.State):
    def __init__(self):
        smach.State.__init__(self, outcomes=['outcome1', 'outcome2', 'preempted'], input_keys=['current_task'], output_keys=['current_task'])

    def execute(self, userdata):
        userdata.current_task = 'TASK2'
        counter = 0
        rospy.loginfo('Executing TaskState2')
        while counter < 3:
            rospy.loginfo(f'Counter: {counter}')
            counter += 1
            rospy.sleep(1.0)
        
        if self.preempt_requested():
            rospy.loginfo('TaskState2: Preempted by emergency stop service.')
            self.service_preempt()
            return 'preempted'

        if counter > 2:
            rospy.loginfo('TaskState2 completed with outcome1.')
            return 'outcome1'
        else:
            rospy.loginfo('TaskState2 completed with outcome2.')
            return 'outcome2'

# 定义紧急处理状态
class EmergencyStopState(smach.State):
    def __init__(self):
        smach.State.__init__(self, outcomes=['resume'], input_keys=['current_task'])

    def execute(self, userdata):
        rospy.logwarn('Emergency Stop State: Stopping all operations.')

        # 创建本地服务客户端来监听取消紧急停车的请求
        rospy.wait_for_service('emergency_stop')
        emergency_stop_service = rospy.ServiceProxy('emergency_stop', SetBool)

        while not rospy.is_shutdown():
            try:
                response = emergency_stop_service(False)  # 尝试发送取消请求
                if response.success and "Emergency stop canceled" in response.message:
                    rospy.loginfo('Emergency Stop Canceled!')
                    return 'resume'
            except rospy.ServiceException as e:
                rospy.logerr(f"Service call failed: {e}")

            rospy.sleep(1.0)  # 模拟等待紧急停止命令或取消命令

# 定义紧急停车服务回调函数
def emergency_stop_service_callback(req, ud):
    if req.data:
        rospy.logwarn('Emergency Stop Service Requested!')
        ud.emergency_stop_flag = True
        sm.request_preempt()  # 请求抢占状态机
        return SetBoolResponse(success=True, message="Emergency stop initiated.")
    else:
        ud.emergency_stop_flag = False
        return SetBoolResponse(success=True, message="Emergency stop canceled.")

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

    # 创建一个状态机并初始化用户数据
    sm = smach.StateMachine(outcomes=['succeeded', 'aborted'])
    sm.userdata.current_task = None
    sm.userdata.emergency_stop_flag = False

    with sm:
        # 添加任务状态到状态机
        smach.StateMachine.add('TASK1', TaskState1(),
                               transitions={'succeeded': 'TASK2',
                                            'preempted': 'EMERGENCY_STOP'})

        smach.StateMachine.add('TASK2', TaskState2(),
                               transitions={'outcome1': 'succeeded',
                                            'outcome2': 'succeeded',
                                            'preempted': 'EMERGENCY_STOP'})

        # 添加紧急处理状态到状态机，并设置恢复逻辑
        def resume_state(ud):
            if ud.current_task == 'TASK1':
                return 'TASK1'
            elif ud.current_task == 'TASK2':
                return 'TASK2'
            else:
                return 'TASK1'  # 默认回到 TASK1 或者其他处理方式

        smach.StateMachine.add('EMERGENCY_STOP', EmergencyStopState(),
                               transitions={'resume': lambda ud: resume_state(ud)})

    # 创建并启动 Introspection Server（用于可视化调试）
    sis = smach_ros.IntrospectionServer('server_name', sm, '/SM_ROOT')
    sis.start()

    # 创建紧急停车服务，并传递用户数据
    rospy.Service('emergency_stop', SetBool, lambda req: emergency_stop_service_callback(req, sm.userdata))

    # 执行状态机
    outcome = sm.execute()

    # 停止 Introspection Server
    sis.stop()

if __name__ == '__main__':
    main()

# 监测话题
# 故障诊断？紧急停车（不太行，不是一直存在）
import rospy
import smach
import smach_ros
from std_msgs.msg import Bool

# 定义一个回调函数，用于处理收到的消息
def sensor_monitor_cb(ud, msg):
    print(msg.data)
    # 如果收到的消息为 True，则返回 'valid' 结果
    if msg.data:
        rospy.loginfo("Sensor data is valid: %s", msg.data)
        ud.sensor_valid = True
        # userdata.sensor_valid = True
        return 'valid'
    else:
        rospy.loginfo("Waiting for valid sensor data...")
        ud.sensor_valid = False
        return 'invalid'

# 定义一个简单的处理数据状态
def process_data_cb(userdata):
    rospy.loginfo('Executing PROCESS_DATA state')
    rospy.loginfo(f"Processing sensor data: {userdata.sensor_valid}")
    return 'succeeded'

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

    # 创建一个状态机
    sm = smach.StateMachine(outcomes=['succeeded', 'aborted'])

    with sm:
        # 添加 MonitorState 到状态机
        smach.StateMachine.add('MONITOR_SENSOR',smach_ros.MonitorState("/sensor_data", Bool, sensor_monitor_cb,max_checks=1, input_keys=['sensor_valid'], output_keys=['sensor_valid']),
            transitions={'valid': 'PROCESS_DATA',  # 数据有效，转到处理状态
                         'invalid': 'MONITOR_SENSOR',  # 继续等待有效数据
                         'preempted': 'aborted'}       # 状态被抢占
        )

        smach.StateMachine.add('PROCESS_DATA',smach.CBState(process_data_cb, input_keys=['sensor_valid'], outcomes=['succeeded']),
            transitions={'succeeded': 'succeeded'}
        )

    # 创建并启动 Introspection Server（用于可视化调试）
    sis = smach_ros.IntrospectionServer('server_name', sm, '/SM_ROOT')
    sis.start()

    # 执行状态机
    outcome = sm.execute()

    # 停止 Introspection Server
    rospy.spin()
    sis.stop()

if __name__ == '__main__':
    main()