Ask Your Question
0

smach state transition latency

asked 2011-04-20 05:27:01 -0600

updated 2011-04-20 05:36:54 -0600

I've run across some curious behavior within smach, and I was curious if others have observed similar issues.

The issue arises when using a Concurrency container _and_ the smach_viewer. In essence, when running my state machine with smach_viewer and without concurrency, the state transitions are almost instantaneous. The same applies when running without the smach_viewer and with the concurrency section. However, when I run both the concurrency container and smach_viewer simultaneously, _all_ state transitions take 7+ seconds. If I start off without the viewer, the transitions are instantaneous, but degrade as soon as the viewer launches.

Any thoughts or suggestions?

PS -- Occasionally I get the following smach_viewer error (though I believe it is unrelated, as it occurs even when everything functions normally):

[ERROR] [WallTime: 1303127819.346248] bad callback: <bound method="" smachviewerframe._status_msg_update="" of="" <__main__.smachviewerframe;="" proxy="" of="" <swig="" object="" of="" type="" 'wxframe="" *'="" at="" 0x953f420=""> >>
Traceback (most recent call last):
  File "/opt/ros/diamondback/stacks/ros_comm/clients/rospy/src/rospy/topics.py", line 563, in _invoke_callback
    cb(msg)
  File "/opt/ros/diamondback/stacks/executive_smach_visualization/smach_viewer/smach_viewer.py", line 659, in _status_msg_update
    if container.update_status(msg):
  File "/opt/ros/diamondback/stacks/executive_smach_visualization/smach_viewer/smach_viewer.py", line 146, in update_status
    self._local_data._data = pickle.loads(msg.local_data)
  File "/usr/lib/python2.6/pickle.py", line 1374, in loads
    return Unpickler(file).load()
  File "/usr/lib/python2.6/pickle.py", line 858, in load
    dispatch[key](self)
  File "/usr/lib/python2.6/pickle.py", line 1090, in load_global
    klass = self.find_class(module, name)
  File "/usr/lib/python2.6/pickle.py", line 1124, in find_class
    __import__(module)
ImportError: No module named geometry_msgs.msg._PoseStamped
edit retag flag offensive close merge delete

2 Answers

Sort by ยป oldest newest most voted
1

answered 2011-04-22 04:24:15 -0600

Wim gravatar image

As Travis discovered, this problem occurs when you use large userdata structures in Smach. These userdata structures are sent to the smach_viewer over ROS, taking up a long of cycles to do the serialization and sending. Smach could be smarter about sending userdata to the smach_viewer.

edit flag offensive delete link more
1

answered 2011-04-21 23:04:17 -0600

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

Stats

Asked: 2011-04-20 05:27:01 -0600

Seen: 604 times

Last updated: Apr 22 '11