ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange |
1 | initial version |
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()