smach state transition latency

edit

I generated a simple test program that uses concurrency in a manner similar to my problematic code (see below). This test program has no latency issues whatsoever.

Regardless, I still see this issue in my large state machine. Without the concurrency block, I never experience the latencies, but as soon as it is put in place the latencies jump to ~7 seconds. The two preemptable nodes are both Python classes that inherit from Smach.State and both use tf and make extensive service calls. My "main" state is a SimpleActionState. I think I'm just going to re-architect the code to avoid the problem. Hopefully no one else runs into similar issues.

#! /usr/bin/python
import roslib
roslib.load_manifest('rfid_demos')
import rospy

import smach
import tf
from smach_ros import SimpleActionState, ServiceState, IntrospectionServer
import actionlib

class QuickTestPreempt( smach.State ):
    def __init__( self, name, plus ):
        smach.State.__init__( self, outcomes = ['preempted'] )
        self.count = 0
        self.plus = plus
        self.name = name

    def execute( self, userdata ):
        r = rospy.Rate( 3 )
        while not smach.State.preempt_requested( self ):
            self.count += self.plus
            rospy.logout( '%s at count %3.2f' % (self.name, self.count))
            r.sleep()
        return 'preempted'

class QuickTestMain( smach.State ):
    def __init__( self, name ):
        smach.State.__init__( self, outcomes = ['succeeded','aborted','preempted'] )
        self.count = 0
        self.name = name

    def execute( self, userdata ):
        r = rospy.Rate( 1 )
        for i in xrange( 5 ):
            self.count += 1
            rospy.logout( '%s at count %3.2f' % (self.name, self.count))
            r.sleep()
        return 'succeeded'

class PrintStr(smach.State):
    def __init__(self, ins = 'Hello'):
        smach.State.__init__(self, outcomes=['succeeded', 'aborted', 'preempted'])
        self.ins = ins

    def execute(self, userdata):
        rospy.logout( 'Executing PrintStr: %s' % self.ins )
        return 'succeeded'


def smc():
    smc = smach.Concurrence( outcomes=['succeeded', 'aborted', 'preempted'],
                            default_outcome = 'aborted',
                            outcome_map = {'succeeded': {'MAIN':'succeeded'},
                                           'aborted': {'MAIN':'aborted'},
                                           'preempted': {'MAIN':'preempted'}},
                            child_termination_cb = lambda arg: True )

    # Open the container

    with smc:
        smach.Concurrence.add('MAIN', QuickTestMain('main'))
        smach.Concurrence.add('ARG', QuickTestPreempt('arg',3.0))
        smach.Concurrence.add('BAH', QuickTestPreempt('bah',2.4))

    return smc

if __name__ == '__main__':
    rospy.init_node( 'tmp_test')

    sm = smach.StateMachine(outcomes=['succeeded','aborted','preempted'])

    with sm:
        smach.StateMachine.add( 'PS1', PrintStr(), transitions={'succeeded':'SMC'})
        smach.StateMachine.add( 'SMC', smc(), transitions={'succeeded':'PS2'})
        smach.StateMachine.add( 'PS2', PrintStr(), transitions={'succeeded':'PS3'})
        smach.StateMachine.add( 'PS3', PrintStr(), transitions={'succeeded':'succeeded'})

    sis = IntrospectionServer( 'sm_test', sm, '/SM_TEST' )
    sis.start()
    rospy.sleep( 5 )

    sm.execute()
    sis.stop()