一、SMACH容器
1.1 状态机容器
1.1.1 创建状态机容器
首先引入状态机容器
from smach import StateMachine
由于SMACH状态机还提供状态接口，因此必须在构造时指定其结果和用户数据交互。
sm = StateMachine(outcomes = ['outcome1', 'outcome2'],
input_keys = ['input1', 'input2'],
output_keys = ['output1' , 'output2'])
与SMACH状态接口类似，输入键和输出键是可选的。构造函数签名是：
__init__(self, outcomes, input_keys=[], output_keys = [])
1.1.2 添加状态
向状态机添加状态时，首先指定要向其添加状态的状态机。这可以通过使用Python的“with”语句来实现。你可以把它想象成“打开”容器进行施工。它创建了一个上下文，在该上下文中，所有后续的add*调用都将应用于打开的容器。
例如，要向名为“sm”的状态机添加两个状态，可以编写以下代码：
with sm:
StateMachine.add('FOO',
FooState(),
{'outcome2':'FOO',
'outcome3':'BAR'})
StateMachine.add('BAR',
BarState(),
{'outcome3':'FOO',
'outcome4':'outcome4'})
上面的示例添加了两个标记为“FOO”和“BAR”的状态，分别为“FooState”和“BarState”类型。静态add方法有可选参数。add方法的签名是：
add(label, state, transitions=None, remapping=None)
1.2 并发容器
from smach import Concurrence
1.2.1 指定并发结果
1）并发结果图
SMACH并发的结果映射指定了基于其子级的结果确定并发结果的策略。具体来说，map是一个字典，其中键是并发的潜在结果，值是将子标签映射到子结果的字典。一旦并发中的所有状态都已终止，如果满足其中一个子结果映射，则并发将返回其关联的结果。如果没有满足任何映射，并发将返回其默认结果。
cc = Concurrence(outcomes = ['outcome1', 'outcome2'],
default_outcome = 'outcome1',
input_keys = ['sm_input'],
output_keys = ['sm_output'],
outcome_map = {'succeeded':{'FOO':'succeeded',
'BAR':'outcome2'},
'outcome3':{'FOO':'outcome2'}})
with cc:
Concurrence.add('FOO', Foo())
Concurrence.add('BAR', Bar())
上面的示例指定了以下策略：
- 当’FOO’的结果为’succeed’且’BAR’的结果为’outcome2’时，状态机将以结果为’succeed’退出。
- 当’FOO’有结果“outcome2”时，状态机将以结果“outcome3”退出，与状态栏的结果无关。
1.2.2 回调
如果希望完全控制并发状态机，可以使用它提供的回调、子级终止和结果：
# gets called when ANY child state terminates
def child_term_cb(outcome_map):
# terminate all running states if FOO finished with outcome 'outcome3'
if outcome_map['FOO'] == 'outcome3':
return True
# terminate all running states if BAR finished
if outcome_map['BAR']:
return True
# in all other case, just keep running, don't terminate anything
return False
# gets called when ALL child states are terminated
def out_cb(outcome_map):
if outcome_map['FOO'] == 'succeeded':
return 'outcome1'
else:
return 'outcome2'
# creating the concurrence state machine
sm = Concurrence(outcomes=['outcome1', 'outcome2'],
default_outcome='outcome1',
input_keys=['sm_input'],
output_keys=['sm_output'],
child_termination_cb = child_term_cb,
outcome_cb = out_cb)
with sm:
Concurrence.add('FOO', Foo(),
remapping={'foo_in':'input'})
Concurrence.add('BAR', Bar(),
remapping={'bar_out':'bar_out'})
-
每当一个子状态终止时，就会调用子状态终止。在回调函数中，您可以决定状态机是应该继续运行（返回False），还是应该抢占所有剩余的运行状态（返回True）。
-
当最后一个子状态终止时，将调用一次结果_cb。此回调返回并发状态机的结果。
1.3 序列容器
1.3.1 创建序列容器
from smach import Sequence
容器序列有其在构造时指定的结果，以及用于自动转换的“连接器_结果”。构造函数签名是：
__init__(self, outcomes, connector_outcome):
1.3.2 添加状态
向序列添加状态与向容器添加状态相同。
但是，每个添加的状态都会收到一个从它到后面添加的状态的额外转换。过渡将遵循该容器建造时规定的结果。
如果dictionary mapping参数中给出的某个转换为’Sequence.add（）'遵循构造函数中指定的连接器结果，提供的转换将覆盖自动生成的连接器转换。
1.3.3 举例
例如，当创建一系列动作状态时，不需要转换映射。所有行动状态都有通常的三重结果，序列也需要有三重结果：
sq = Sequence(
outcomes = ['succeeded','aborted','preempted'],
connector_outcome = 'succeeded')
with sq:
Sequence.add('MOVE_ARM_GRAB_PRE', MoveVerticalGripperPoseActionState())
Sequence.add('MOVE_GRIPPER_OPEN', MoveGripperState(GRIPPER_MAX_WIDTH))
Sequence.add('MOVE_ARM_GRAB', MoveVerticalGripperPoseActionState())
Sequence.add('MOVE_GRIPPER_CLOSE', MoveGripperState(grab_width))
Sequence.add('MOVE_ARM_GRAB_POST', MoveVerticalGripperPoseActionState())
1.4迭代容器
1.4.1 概述
迭代器允许您循环一个或多个状态，直到满足成功条件。本教程演示如何使用迭代器将数字列表排序为偶数和赔率。
import roslib; roslib.load_manifest('smach')
roslib.load_manifest('smach_ros')
import rospy
import smach
from smach import Iterator, StateMachine, CBState
from smach_ros import ConditionState, IntrospectionServer
def construct_sm():
sm = StateMachine(outcomes = ['succeeded','aborted','preempted'])
sm.userdata.nums = range(25, 88, 3)
sm.userdata.even_nums = []
sm.userdata.odd_nums = []
with sm:
tutorial_it = Iterator(outcomes = ['succeeded','preempted','aborted'],
input_keys = ['nums', 'even_nums', 'odd_nums'],
it = lambda: range(0, len(sm.userdata.nums)),
output_keys = ['even_nums', 'odd_nums'],
it_label = 'index',
exhausted_outcome = 'succeeded')
with tutorial_it:
container_sm = StateMachine(outcomes = ['succeeded','preempted','aborted','continue'],
input_keys = ['nums', 'index', 'even_nums', 'odd_nums'],
output_keys = ['even_nums', 'odd_nums'])
with container_sm:
#test wether even or odd
StateMachine.add('EVEN_OR_ODD',
ConditionState(cond_cb = lambda ud:ud.nums[ud.index]%2,
input_keys=['nums', 'index']),
{'true':'ODD',
'false':'EVEN' })
#add even state
@smach.cb_interface(input_keys=['nums', 'index', 'even_nums'],
output_keys=['odd_nums'],
outcomes=['succeeded'])
def even_cb(ud):
ud.even_nums.append(ud.nums[ud.index])
return 'succeeded'
StateMachine.add('EVEN', CBState(even_cb),
{'succeeded':'continue'})
#add odd state
@smach.cb_interface(input_keys=['nums', 'index', 'odd_nums'],
output_keys=['odd_nums'],
outcomes=['succeeded'])
def odd_cb(ud):
ud.odd_nums.append(ud.nums[ud.index])
return 'succeeded'
StateMachine.add('ODD', CBState(odd_cb),
{'succeeded':'continue'})
#close container_sm
Iterator.set_contained_state('CONTAINER_STATE',
container_sm,
loop_outcomes=['continue'])
#close the tutorial_it
StateMachine.add('TUTORIAL_IT',tutorial_it,
{'succeeded':'succeeded',
'aborted':'aborted'})
return sm
def main():
rospy.init_node("iterator_tutorial")
sm_iterator = construct_sm()
# Run state machine introspection server for smach viewer
intro_server = IntrospectionServer('iterator_tutorial',sm_iterator,'/ITERATOR_TUTORIAL')
intro_server.start()
outcome = sm_iterator.execute()
rospy.spin()
intro_server.stop()
if __name__ == "__main__":
main()
程序解释
从构造迭代器开始，构造函数采用以下参数：
__init__(self, outcomes, input_keys, output_keys, it=[], it_label='it_data', exhausted_outcome='exhausted')
在本例中，结果现在包括preempted，这是迭代器的默认结果。it参数是要迭代的对象列表，it_标签是保存it列表中项目的当前值的键。耗尽的参数应该设置为首选状态机结果，在本例中，当迭代器在it列表中循环完成时，迭代器结果成功。
tutorial_it = Iterator(outcomes = ['succeeded','preempted','aborted'],
input_keys = ['nums', 'even_nums', 'odd_nums'],
it = lambda: range(0, len(sm.userdata.nums)),
output_keys = ['even_nums', 'odd_nums'],
it_label = 'index',
exhausted_outcome = 'succeeded')
现在添加一个容器，并创建用于将列表排序为偶数和奇数的状态。
with tutorial_it:
container_sm = StateMachine(outcomes = ['succeeded','preempted','aborted','continue'],
input_keys = ['nums', 'index', 'even_nums', 'odd_nums'],
output_keys = ['even_nums', 'odd_nums'])
with container_sm:
#test wether even or odd
StateMachine.add('EVEN_OR_ODD',
ConditionState(cond_cb = lambda ud:ud.nums[ud.index]%2,
input_keys=['nums', 'index']),
{'true':'ODD',
'false':'EVEN' })
#add even state
@smach.cb_interface(input_keys=['nums', 'index', 'even_nums'],
output_keys=['odd_nums'],
outcomes=['succeeded'])
def even_cb(ud):
ud.even_nums.append(ud.nums[ud.index])
return 'succeeded'
StateMachine.add('EVEN', CBState(even_cb),
{'succeeded':'continue'})
#add odd state
@smach.cb_interface(input_keys=['nums', 'index', 'odd_nums'],
output_keys=['odd_nums'],
outcomes=['succeeded'])
def odd_cb(ud):
ud.odd_nums.append(ud.nums[ud.index])
return 'succeeded'
StateMachine.add('ODD', CBState(odd_cb),
{'succeeded':'continue'})
将容器添加到迭代器：
#close container_sm
Iterator.set_contained_state('CONTAINER_STATE',
container_sm,
loop_outcomes=['continue'])
通过将迭代器添加到顶级状态机来完成：
#close the tutorial_it
StateMachine.add('TUTORIAL_IT',tutorial_it,
{'succeeded':'succeeded',
'aborted':'aborted'})
1.5 使用actionlib包装容器
1.5.1 包装 SMACH 状态
SMACH提供了名为ActionServerWrapper的顶级容器。这一个类播发actionlib操作服务器。当操作服务器收到目标时，其包含的状态将变为活动状态，而不是由父级执行。因此，此容器不会从smach继承。状态基类，不能放在另一个容器中。
action server包装器可以将action server接收到的目标消息注入到包含状态，并在终止时从该状态提取结果消息。在构造操作服务器包装器时，用户指定哪些状态机结果对应于成功、中止或抢占的结果。
考虑这个例子，它将一个SMACH状态机封装为一个动作：
import rospy
from smach import StateMachine
from smach_ros import ActionServerWrapper
# Construct state machine
sm = StateMachine(outcomes=['did_something',
'did_something_else',
'aborted',
'preempted'])
with sm:
### Add states in here...
# Construct action server wrapper
asw = ActionServerWrapper(
'my_action_server_name', MyAction,
wrapped_container = sm,
succeeded_outcomes = ['did_something','did_something_else'],
aborted_outcomes = ['aborted'],
preempted_outcomes = ['preempted'] )
# Run the server in a background thread
asw.run_server()
# Wait for control-c
rospy.spin()
1.5.2 让目标/结果消息进入/退出包含状态
上面的代码将调用 sm.execute()，但它不会将目标加载到包含的状态机中，也不会提取结果。为了做这些事情，您需要告诉动作服务器包装器在 SMACH 的上下文中它应该调用什么目标和结果消息。您可以将操作服务器包装器构造调用替换为以下内容：
# Construct action server wrapper
asw = ActionServerWrapper(
'my_action_server_name', MyAction, sm,
['did_something','did_something_else'], ['aborted'], ['preempted'],
goal_key = 'my_awesome_goal',
result_key = 'egad_its_a_result' )
关键字参数goal_key和result_key是ActionServerWrapper上下文中的 SMACH 用户数据键。与任何其他容器一样，这意味着包装器的包含状态（在本例中为状态机sm）将在调用其 execute() 方法时接收对此用户数据结构的引用。与嵌套状态机中的范围之间传递用户数据的方式类似，在这种情况下，您也需要在状态机sm中设置这些键标识符。
为了从父级复制密钥，您可以将状态机sm的构造调用替换为：
# Construct state machine
sm = StateMachine(
outcomes=['did_something','did_something_else','aborted','preempted'],
input_keys = ['my_awesome_goal'],
output_keys = ['egad_its_a_result'])
完成此操作后，您可以从添加到sm的任何状态访问这些密钥。有关更多目标/结果策略，请参阅ActionServerWrapper API文档。
二.SMACH 状态
2.1 通用状态
from smach import State
创建状态的最通用（但通常效率最低）的方法是从“状态”基类派生。你几乎可以在状态的执行方法中做任何你想做的事情。知道 SMACH 带有一个有用的状态库，您可以使用它，而无需编写整个自定义状态。
class Foo(smach.State):
def __init__(self, outcomes=['outcome1', 'outcome2']):
# Your state initialization goes here
def execute(self, userdata):
# Your state execution goes here
if xxxx:
return 'outcome1'
else:
return 'outcome2'
2.2 CBState
from smach import CBState
2.2.1 描述
此状态在执行状态时仅执行单个回调。这对于在状态中执行任意代码很有用，而无需声明新的状态类。这个类支持使用smach.cb_interface装饰器，它的 API 可以在 这里找到。
CBState 使用至少一个参数调用回调：容器的用户数据。可以在构造时将其他参数和关键字参数提供给 CBState。当执行 CBState 时，这些参数将被传递到回调中。
以下示例将 CBState 添加到 SMACH StateMachine中，该状态机从 userdata 中获取一些参数和变量，并将它们的总和存储在 userdata 键“xyz”中：
@smach.cb_interface(input_keys=['q'],
output_keys=['xyz'],
outcomes=['foo'])
def my_cb(ud, x, y, z):
ud.xyz = ud.q + x + y + z
return 'foo'
...
with sm:
...
StateMachine.add('MY_CB', CBState(my_cb,
cb_args=[10],
cb_kwargs={'z':2,'y':3}),
{'foo':'OTHER_STATE'})
2.3 简单动作状态ROS
见前面
2.4 服务状态（ROS）
from smach_ros import ServiceState
您可以简单地从通用状态调用任何服务，但 SMACH 对调用服务有特定的支持，从而为您节省大量代码！SMACH 提供了一个状态类，作为ROS 服务的代理。状态的实例化采用服务名称、服务类型和一些用于生成服务请求的策略。服务状态的可能结果是“成功”、“抢占”和“中止”。
/!\服务状态几乎与简单动作状态相同。只需将“目标”替换为“请求”，将“结果”替换为“响应”即可。
2.4.1 要求
1）空请求消息
sm = StateMachine(['succeeded','aborted','preempted'])
with sm:
smach.StateMachine.add('TRIGGER_GRIPPER',
ServiceState('service_name',
GripperSrv),
transitions={'succeeded':'APPROACH_PLUG'})
2）固定请求消息
sm = StateMachine(['succeeded','aborted','preempted'])
with sm:
smach.StateMachine.add('TRIGGER_GRIPPER',
ServiceState('service_name',
GripperSrv,
request = GripperSrv(9.0)),
transitions={'succeeded':'APPROACH_PLUG'})
3）来自用书数据的请求
sm = StateMachine(['succeeded','aborted','preempted'])
with sm:
smach.StateMachine.add('TRIGGER_GRIPPER',
ServiceState('service_name',
GripperSrv,
request_slots = ['max_effort',
'position']),
transitions={'succeeded':'APPROACH_PLUG'})
4）请求回调
sm = StateMachine(['succeeded','aborted','preempted'])
with sm:
@smach.cb_interface(input_keys=['gripper_input'])
def gripper_request_cb(userdata, request):
gripper_request = GripperSrv().Request
gripper_request.position.x = 2.0
gripper_request.max_effort = userdata.gripper_input
return gripper_request
smach.StateMachine.add('TRIGGER_GRIPPER',
ServiceState('service_name',
GripperSrv,
request_cb = gripper_request_cb,
input_keys = ['gripper_input']),
transitions={'succeeded':'APPROACH_PLUG'})
2.4.2 回复
1）对用户数据的响应
2）响应回调
2.5 监控状态（ROS）
这是一个使用MonitorState的简单状态机示例。为了从状态 Foo 转换到状态 Bar，向 /sm_reset 主题发送消息：
rostopic pub -1 /sm_reset std_msgs/Empty
1) 代码
#!/usr/bin/env python
import roslib; roslib.load_manifest('smach_MonitorState_example')
import rospy
import smach
import smach_ros
from std_msgs.msg import Empty
class bar(smach.State):
def __init__(self):
smach.State.__init__(self, outcomes=['bar_succeeded'])
def execute(self, userdata):
rospy.sleep(3.0)
return 'bar_succeeded'
def monitor_cb(ud, msg):
return False
def main():
rospy.init_node("monitor_example")
sm = smach.StateMachine(outcomes=['DONE'])
with sm:
smach.StateMachine.add('FOO', smach_ros.MonitorState("/sm_reset", Empty, monitor_cb), transitions={'invalid':'BAR', 'valid':'FOO', 'preempted':'FOO'})
smach.StateMachine.add('BAR',bar(), transitions={'bar_succeeded':'FOO'})
sis = smach_ros.IntrospectionServer('smach_server', sm, '/SM_ROOT')
sis.start()
sm.execute()
rospy.spin()
sis.stop()
if __name__=="__main__":
main()
2) 带注释的代码
def monitor_cb(ud, msg):
return False
参数是用户数据和消息。当我们希望监视器状态终止时，这需要返回 False。在这种情况下，监视器状态将返回“无效”
sm = smach.StateMachine(outcomes=['DONE'])
with sm:
smach.StateMachine.add('FOO',
smach_ros.MonitorState("/sm_reset",
Empty,
monitor_cb),
transitions={'invalid':'BAR',
'valid':'FOO',
'preempted':'FOO'})
请注意，您的MonitorState需要为所有三种可能的结果（“有效”、“无效”、“抢占”）定义转换。
从MonitorState的源代码：
def __init__(self, topic, msg_type, cond_cb, max_checks=-1)
max_checks 参数是在MonitorState将返回“有效”之前可以调用多少次 monitor_cb 的限制。
扫一扫
1858
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
