ROS SMACH示例教程(四)

ROS SMACH示例教程(四)

1. 状态抢占实现

在实际使用中，给定的状态或者容器类并不能满足我们的需求，所以经常需要进行自定义我们的状态类和容器类。在自定义一个新的SMACH状态的过程中主要考虑以下三个方面：

• 交互方式的定义
• 执行程序的实现
• 状态抢占的实现

下面给出一个样例来帮助快速复制粘贴代码来实现一个状态的定义

import roslib; roslib.load_manifest('smach')
import rospy

class FibState(State):
	"""构造State状态"""
	def __init__(self, n):
		State.__init__(self, 
				    	outcomes = ['done' , 'preempted'],
				    	input_keys = [ ],
				    	output_keys = ['fib_result' ])
		self.n = n
	def execute(self , ud):
		""" 计算FIbonacci 数列  """
		f = 0
		f1 = 0
		f2 = 0
		for i in range(self.n):
			# 检查抢占
			if self.service_preempt()
				return 'preempted'
			
			# 计算下一组数字
			f = f1 + f2
			f1 = f2
			f2 = f
			
		# 将包含结果的元组存储在userdata中
		ud.fib_result = (self.n, f)
		return 'done'
		
	def  request_preempt(self):
		""" 重载抢占请求方法只是为了抛出一个Warnning! """		
		State.request_preempt(self)
		rospy.logwarn("Preempted!")

1.1 SMACH接口

SMACH容器通过状态结果和用户数据键与其包含的状态进行交互。为了能够绑定到容器中的目标，需要声明结果，为了跟踪系统中的数据流，需要声明userdata键。这个显式声明允许我们在构造SMACH树时以及执行SMACH树时捕捉错误。

1.2 执行实现

当某个容器的执行进入状态时，将调用其execute()方法。这是一个阻塞并返回一些（注册的）结果标识符（字符串）的方法。一旦此方法退出，状态应被视为休眠或不活动。
如果某个状态对应于一个长时间运行的任务，那么它应该在并发或其他一些并行容器中启动，而不是脱离线程快速退出。

1.3 抢占实现

在上面的例子这样的简单情况下，只需检查preempt_requested()方法即可实现抢占。如果无法做到这一点，可以在这个新的状态类中重载request_preempt()方法。抢占通常会出现在一个单独的线程上，这种方法的阻塞时间不应超过通知任何子线程/进程抢占所需的时间。

2.状态机抢占

使用并发容器和监视器状态实现抢占的状态机示例。

这正是我在试图理解如何在状态机中添加抢占时编写的代码。请不要将其视为最佳实践或任何东西，直到JonBohren或有更多经验的人看到它
											-LauraLindzey

2.1 开始

这里的目标是使状态机中的状态在计算完成或满足特定条件后终止。我们使用并发容器将实现抢占的简单状态和监视状态连接在一起。监视器状态检查是否已收到/sm_reset的消息。
sm状态从SETUP开始，过渡到FOO，然后在FOO和BAR之间循环。FOO是一个并发容器，包含FOO_RESET、MonitorState 和 FOO_CALC（执行长时间计算的简单状态）。如果机器处于FOO状态，并且向/sm_reset发送消息，则FOO_reset将返回，FOO_CALC将被抢占，并且机器将转换为SETUP。否则，如果FOO_CALC完成，FOO_RESET将被抢占，机器将转换为BAR。

2.2 代码

#！/usr/bin/env python
import roslib; roslib.load_manifest('smach_preemption_example')
import rospy
import smach
import smach_ros

class setup(smach.State):
	def __init__(self):
		smach.State.__init__(self, outcomes=['setup_done'])
	def execute(self, userdata):
		rospy.sleep(3.5)
		return 'setup_done'
		
class foo(smach.State):
	def __init__(self):
		smach.State.__init__(self, outcomes= ['foo_succeeded', 'preempted'])
	def execute(self, userdata):
		for idx in range(5):
			if self.preempt_requested():
				print "state foo is being preempted!!!!"
				self.service_preempt()
				return 'preempted'
			rospy.sleep(1.0)
			
	def child_term_cb(outcome_map):
		if outcome_map['FOO_RESET'] == 'fpp_succeeded':
			return 'foo_reset'
		elif outcome['FOO_CALC'] == 'foo_succeeded':
			rerunrn 'foo_done'
		else:
			return 'foo_reset'
	def out_cb(ud, msg):
		return Flase
		
	def main():
		rospy.init_node("preemption_example")
		foo_concurrence = smach.Concurence(outcomes=['foo_done','foo_reset'],
									default_outcome='foo_done',
									child_termination_cb=child_term_cb,
									outcome_cb=out_cb)

		with foo_concurrence:
			smach.Concurrence.add('FOO_CALC', foo())
			smach.Concurrence.add('FOO_RESET', smach_ros.MonitorState("/sm_reset",  Empty, monitor_cb))
		
		sm = smach.StateMachine(outcomes=['DONE'])
		with sm:
			smach.StateMachine.add('SETUP', setup(), transitions={'setup_done':'FOO'})
			smach.StateMachine.add('FOO', foo_concurrence, transitions={'foo_done':'BAR', 'foo_reset':'SETUP'})
			smach.StateMachine.add('BAR',foo(), transitions={'foo_succeeded':'FOO', 'preempted':'SETUP'})
		
		sis = smach_ros.IntrospectionServer('smach_server', sm, '/SM_ROOT')
		sis.start()
		sm.execute()
		rospy.spin()
		sis.stop()
		
if __name__=="__main__":
	main()			

2.3 代码解析

	foo_concurrence = smach.Concurence(outcomes=['foo_done','foo_reset'],
									default_outcome='foo_done',
									child_termination_cb=child_term_cb,
									outcome_cb=cou_cb)

上述代码定义了一个foo_concurence的并发状态，并且初始化它的输入、默认输出和回调函数

	with foo_concurrence:
			smach.Concurrence.add('FOO_CALC', foo())
			smach.Concurrence.add('FOO_RESET', smach_ros.MonitorState("/sm_reset",  Empty, monitor_cb))		

上述代码在foo_currence中添加了两个状态FOO_CALC和FOO_RESET，并且将自定义的foo()和MonitorState与状态机链接起来

		sm = smach.StateMachine(outcomes=['DONE'])
		with sm:
			smach.StateMachine.add('SETUP', setup(), transitions={'setup_done':'FOO'})
			smach.StateMachine.add('FOO', foo_concurrence, transitions={'foo_done':'BAR', 'foo_reset':'SETUP'})
			smach.StateMachine.add('BAR',foo(), transitions={'foo_succeeded':'FOO', 'preempted':'SETUP'})

上述代码添加了三个状态，分别对应setup(),foo_concurrence,foo()三个状态机。

3.使用用户定义的回调函数编写自定义状态类

有时候你想创建一个像smach.SimpleActionState这样的状态类，用自定义的回调函数解析。如果这些回调函数改变了状态的SMACH接口（结果、用户数据键），那么回调需要用这些信息进行注释。

SMACH提供了一个装饰器，可以将SMACH接口附加到函数。这包括用户数据输入键、输出键和状态结果。可扩展状态设计的一种常见模式是使用在各种事件上调用的回调函数。例如，SimpleActionState可以调用回调函数来生成目标消息或处理结果消息，而MonitorState在每次收到有关指定主题的消息时都会调用回调函数。

如果一个状态定义了一个可以接受回调函数的接口，那么它应该支持这个装饰器模型，这样做就像检查SMACH接口方法的存在一样简单。这些功能通过smach.util模块提供。

3.1 单一用户回调

下面的代码展示了如何将回调函数提供的SMACH接口集成到状态的接口中。这个状态只接收一个回调函数，并在状态执行时调用它。

class MyState(smach.State):
	def __init__(self, cb):
		smach.State.__init__(self, outcomes=['done'])
		
		self._cb = cb
		if cb and smach.has_smach_interface(cb):
			self.register_input_keys(cb.get_registered_inpiut_keys())
			self.register_output_keys(cb.get_registered_output_keys())
			self.register_outcomes(cb.get_registered_outcomes())
	...
	
	def execute(self, ud):
		# 回调函数调用
		cb_outcome = self._cb(ud)
		
		# 如果回调函数调用了dev返回回调函数的结果，否则返回‘done’
		if cb_outcome:
			return cb_outcome
			
		return 'done'

3.2 多个用户定义的回调函数

如果提供了多个回调函数，状态类的实现应该确保添加具有不同接口的回调函数不会产生任何问题。这是通过在向回调传递userdata时使用userdata重映射来实现的。
以下状态将回调函数列表作为参数，并存储每个回调的输入和输出键。然后，当在执行此状态时调用每个回调函数时，它们将只接收对其声明的输入和输出键的访问。这样可以防止执行一个回调函数时被另一个回调意外启动。

class   MyState(smach.State):
	def __init__(self, cd_list):
		smach.State.__init__(self, outcomes=['done']):
			
			self._cbs  =  cb_lists
			for cb in cb_list:
				if cb and smach.has_smach_interface(cb):
					self._cb_input_keys.append(cb.get_registeres_input_keys())
					self._cb_output_keys.append(cb.get_registeres_output_keys())
					self.register_input_keys(self._cb_input_keys[-1])
					self.register_output_keys(self._cb_output_keys[-1])
					self.register_outcomes(self._cb_input_keys[-1]) 
					
	def execute(self, ud):
		# 回调函数
		for (cn, ik, ok) in zip(self._cbs,
							self._cb_input_keys,
							self._cb_output_keys):
			#  使用有限的用户数据调用回调
			cb_outcome = self._cb(smach.Remapper(ud, ik, ok, {}))
			
			# 如果回调返回结果，则将其作为状态结果返回
			if cb_outcome:
				return cb_outcome
		
		return 'done'

主要修改的代码是

self._cbs  =  cb_lists

上述代码将回调函数列表返回到目前的回调函数中。

	for cb in cb_list:
		if cb and smach.has_smach_interface(cb):
			self._cb_input_keys.append(cb.get_registeres_input_keys())
			self._cb_output_keys.append(cb.get_registeres_output_keys())
			self.register_input_keys(self._cb_input_keys[-1])
			self.register_output_keys(self._cb_output_keys[-1])
			self.register_outcomes(self._cb_input_keys[-1]) 

上述将回调函数逐个循环，将每个回调函数的输入、输出添加在输入列表的最后，并且将列表最后添加的输入、输出值添加状态的输入和输出中。

	# 回调函数
	for (cn, ik, ok) in zip(self._cbs,
						self._cb_input_keys,
						self._cb_output_keys):
		#  使用有限的用户数据调用回调
		cb_outcome = self._cb(smach.Remapper(ud, ik, ok, {}))

其中使用cn，ik，ok循环迭代了self._cbs, self._cb_input_keys, self._cb_output_keys，并且使用Remapper进行了参数映射。