ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange |
1 | initial version |
I have trouble understanding your question (specific errors would help).
Shown below is a working example based on what I think you are trying to accomplish:
#!/usr/bin/env python
import rospy
import smach
class Baz(object):
def __init__(self, baz):
self.baz = baz
def __repr__(self):
return '{}({})'.format(self.__class__, self.baz)
class Qux(object):
def __init__(self, qux):
self.qux = [qux]
def __repr__(self):
return '{}({})'.format(self.__class__, self.qux[0])
class Foo(smach.State):
def __init__(self):
smach.State.__init__(self,
outcomes=['continue','done'],
input_keys=['foo_in'],
output_keys=['foo_out'])
self.execute_cnt = 0
def execute(self, userdata):
if rospy.is_shutdown():
return 'done'
else:
rospy.loginfo('Executing state Foo')
rospy.loginfo(" received userdata.foo_in: {}".format(userdata.foo_in))
tmp = Baz(self.execute_cnt)
rospy.loginfo(" sending userdata.foo_out: {}".format(tmp))
userdata.foo_out = tmp
rospy.sleep(1.0)
self.execute_cnt += 1
return 'continue'
class Bar(smach.State):
def __init__(self):
smach.State.__init__(self,
outcomes=['continue', 'done'],
input_keys=['bar_in'],
output_keys=['bar_out'])
self.execute_cnt = 0
def execute(self, userdata):
if rospy.is_shutdown():
return 'done'
else:
rospy.loginfo('Executing state Bar')
rospy.loginfo(" received userdata.bar_in: {}".format(userdata.bar_in))
tmp = Qux(self.execute_cnt)
rospy.loginfo(" sending userdata.bar_out: {}".format(tmp))
userdata.bar_out = tmp
rospy.sleep(3.0)
self.execute_cnt += 1
return 'continue'
def main():
rospy.init_node('smach_example_state_machine')
# Create a SMACH state machine
sm = smach.StateMachine(outcomes=['exit'])
sm.userdata.connector = None
# Open the container
with sm:
# Add states to the container
smach.StateMachine.add('FOO', Foo(),
transitions={'continue': 'BAR',
'done': 'exit'},
remapping={'foo_in': 'connector',
'foo_out': 'connector'})
smach.StateMachine.add('BAR', Bar(),
transitions={'continue': 'FOO',
'done': 'exit'},
remapping={'bar_in': 'connector',
'bar_out': 'connector'})
# Execute SMACH plan
outcome = sm.execute()
if __name__ == '__main__':
main()