Ask Your Question
0

How to pass mutable input and output keys in SMACH

asked 2019-01-30 10:15:57 -0500

Tex gravatar image

updated 2019-01-30 12:46:55 -0500

Hello,

I am currently using the melodic distro. Here are my ROS env vars:

ROS_ETC_DIR=/opt/ros/melodic/etc/ros
ROS_ROOT=/opt/ros/melodic/share/ros
ROS_MASTER_URI=http://localhost:11311
ROS_VERSION=1
ROS_PYTHON_VERSION=2
ROS_PACKAGE_PATH=/opt/ros/melodic/share
ROSLISP_PACKAGE_DIRECTORIES=
ROS_DISTRO=melodic

My question is more conceptual than, "I need help fixing this error," so my environment information may not be completely relevant.

I am having a hard time finding any documentation on passing mutable user data between different state machines. In fact, the only real documentation I can find for SMACH is (understandably) the ros wiki page. I have linked the only page I can find on user data. On this topic, it says

If you require a mutable input object, you must specify the same key in both input_keys and output_keys

Okay, but what if each state machine accepts and outputs a defined object that I defined that is not a string, int, etc (immutable objects)? What if, say, state machine FOO expects some Foo() object, and send a Bar() object to state machine BAR? Is there any documentation (examples preferred) on how to do that?

So, instead of something like this:

class Foo(smach.State):
    def __init__(self):
        smach.State.__init__(self, 
                             outcomes=['outcome1','outcome2'],
                             input_keys=['foo_counter_in'],
                             output_keys=['foo_counter_out'])

    def execute(self, userdata):
        rospy.loginfo('Executing state FOO')
        if userdata.foo_counter_in < 3:
            userdata.foo_counter_out = userdata.foo_counter_in + 1
            return 'outcome1'
        else:
            return 'outcome2'


# define state Bar
class Bar(smach.State):
    def __init__(self):
        smach.State.__init__(self, 
                             outcomes=['outcome1'],
                             input_keys=['bar_counter_in'])

    def execute(self, userdata):
        rospy.loginfo('Executing state BAR')
        rospy.loginfo('Counter = %f'%userdata.bar_counter_in)        
        return 'outcome1'


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

    # Create a SMACH state machine
    sm = smach.StateMachine(outcomes=['outcome4'])
    sm.userdata.sm_counter = 0

    # Open the container
    with sm:
        # Add states to the container
        smach.StateMachine.add('FOO', Foo(), 
                               transitions={'outcome1':'BAR', 
                                            'outcome2':'outcome4'},
                               remapping={'foo_counter_in':'sm_counter', 
                                          'foo_counter_out':'sm_counter'})
        smach.StateMachine.add('BAR', Bar(), 
                               transitions={'outcome1':'FOO'},
                               remapping={'bar_counter_in':'sm_counter'})


    # Execute SMACH plan
    outcome = sm.execute()


if __name__ == '__main__':
    main()

I would like to have something like this (but apparently it won't work):

class Foo(smach.State):
    def __init__(self):
        smach.State.__init__(self, 
                             outcomes=['outcome1','outcome2'],
                             input_keys=['foo_in'],
                             output_keys=['foo_out'])

    def execute(self, userdata):
        rospy.loginfo('Executing state FOO')
        userdata.foo_out = BarObj()
        return 'outcome1'


# define state Bar
class Bar(smach.State):
    def __init__(self):
        smach.State.__init__(self, 
                             outcomes=['outcome1'],
                             input_keys=['bar_in'],
                             output_keys=['bar_out'])

    def execute(self, userdata):
        userdata.bar_out = SomethingElse()       
        return 'outcome1'


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

    # Create a SMACH state machine
    sm = smach.StateMachine(outcomes=['outcome4'])
    sm.userdata.sm_input = FooObj()

    # Open the container
    with sm:
        # Add states to the container
        smach.StateMachine.add('FOO', Foo(), 
                               transitions={'outcome1':'BAR', 
                                            'outcome2':'outcome4'},
                               remapping={'foo_in':'sm_input', 
                                          'foo_out':'foo_output'})
        smach.StateMachine.add('BAR', Bar(), 
                               transitions={'outcome1':'FOO'},
                               remapping={'bar_in':'foo_output'
                                          'bar_out' : 'sm_out'})


    # Execute SMACH plan
    outcome = sm.execute()


if __name__ == '__main__':
    main()

What I want to happen here is that the StateMachine SM that has two state machines, Foo and Bar, should have input keys ... (more)

edit retag flag offensive close merge delete

Comments

1

So... how do I make input and output keys immutable?

mutable or immutable?

but apparently it won't work

apparently, or are you getting errors? If so, please share them.

gvdhoorn gravatar imagegvdhoorn ( 2019-01-30 12:45:46 -0500 )edit

Sorry, mutably*

Tex gravatar imageTex ( 2019-01-30 12:46:40 -0500 )edit

1 Answer

Sort by ยป oldest newest most voted
1

answered 2019-01-30 15:41:05 -0500

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()
edit flag offensive delete link more

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower

Stats

Asked: 2019-01-30 10:15:57 -0500

Seen: 66 times

Last updated: Jan 30