Using same data in different states of SMACH Concurrent-container

Question

Let's say we have a concurrent SMACH container sm_con which includes two state machines SM1 and SM2. I need to find a way for SM1 to continuously update some data and for SM2 to access (and eventually also modify) the same data. I thought about solving this by passing the userdata of sm_con to SM1 and SM2 as input- and output-keys hoping that if SM1 modifies the data it would automatically overwrite sm_cons userdata (kind of like working with pointers in c++) but this doesn't work.

The corresponding code would look like this:

import smach
import smach_ros
import rospy

class st1(smach.State):
    def __init__(self, outcomes=['successful', 'preempted']):
        smach.State.__init__(self, outcomes, input_keys=['in_test'], output_keys=['out_test'])

    def execute(self, userdata):
        if self.preempt_requested():
            self.service_preempt()
            return 'preempted'
        rospy.logerr('test 1: '+str(userdata.in_test))
        userdata.out_test=userdata.in_test+1
        return 'successful'

class st2(smach.State):
    def __init__(self, outcomes=['successful', 'preempted']):
        smach.State.__init__(self, outcomes, input_keys=['in_test'])

    def execute(self, userdata):
        #time.sleep(2)
        if self.preempt_requested():
            self.service_preempt()
            return 'preempted'
        rospy.logerr('test 2: ' + str(userdata.in_test))
        return 'successful'


if __name__=="__main__":
    rospy.init_node('test_state_machine')

    sm_con = smach.Concurrence(outcomes=['success'],
                               default_outcome='success'
                               )
    sm_con.userdata.testdata = 0
    with sm_con:
        sm_1 = smach.StateMachine(outcomes=['success', 'preempted'], input_keys=['testdata'], output_keys=['testdata'])
        with sm_1:
            smach.StateMachine.add('ST1', st1(),
                                   remapping={'in_test': 'testdata', 'out_test': 'testdata'},
                                   transitions={'successful': 'ST1'})

        sm_2 = smach.StateMachine(outcomes=['success', 'preempted'], input_keys=['testdata'])
        with sm_2:
            smach.StateMachine.add('ST2', st2(),
                                   remapping={'in_test':'testdata'},
                                   transitions={'successful': 'ST2'})

        smach.Concurrence.add('SM1', sm_1)
        smach.Concurrence.add('SM2', sm_2)

    # Execute SMACH plan
    outcome = sm_con.execute()
    print('exit-outcome:' + outcome)
    # Wait for ctrl-c to stop the application
    rospy.spin()

Running this code, the output 'test 1: ...' shows that inside SM1 the userdata gets incremented while the output 'test 2: ...' shows that SM2 doesn't access the incremented data as the output remains 0.

How can I modify some data in SM1 and access the modified data in SM2?

Answer 1

I found a workaround for this using mutable objects like described here.

Applied on the code above, it would look like the following:

import smach
import smach_ros
import rospy

class st1(smach.State):
    def __init__(self, outcomes=['successful', 'preempted']):
        smach.State.__init__(self, outcomes, input_keys=['in_test'])

    def execute(self, userdata):
        if self.preempt_requested():
            self.service_preempt()
            return 'preempted'
        rospy.logerr('test 1: '+str(userdata.in_test))
        userdata.in_test[0]=userdata.in_test[0]+1
        return 'successful'

class st2(smach.State):
    def __init__(self, outcomes=['successful', 'preempted']):
        smach.State.__init__(self, outcomes, input_keys=['in_test'])

    def execute(self, userdata):
        #time.sleep(2)
        if self.preempt_requested():
            self.service_preempt()
            return 'preempted'
        rospy.logerr('test 2: ' + str(userdata.in_test[0]))
        return 'successful'


if __name__=="__main__":
    rospy.init_node('test_state_machine')

    sm_con = smach.Concurrence(outcomes=['success'],
                               default_outcome='success'
                               )
    sm_con.userdata.testdata = [0]
    with sm_con:
        sm_1 = smach.StateMachine(outcomes=['success', 'preempted'], input_keys=['testdata'])
        with sm_1:
            smach.StateMachine.add('ST1', st1(),
                                   remapping={'in_test': 'testdata'},
                                   transitions={'successful': 'ST1'})

        sm_2 = smach.StateMachine(outcomes=['success', 'preempted'], input_keys=['testdata'])
        with sm_2:
            smach.StateMachine.add('ST2', st2(),
                                   remapping={'in_test':'testdata'},
                                   transitions={'successful': 'ST2'})

        smach.Concurrence.add('SM1', sm_1)
        smach.Concurrence.add('SM2', sm_2)

    # Execute SMACH plan
    outcome = sm_con.execute()
    print('exit-outcome:' + outcome)
    # Wait for ctrl-c to stop the application
    rospy.spin()

Since this is only a workaround, refer to my corresponding issue-post here for more information.