ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question

Revision history [back]

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()