<s> from director import lcmUtils <EOL> from director import transformUtils <EOL> import PythonQt <EOL> import bot_core as lcmbotcore <EOL> from director . simpletimer import SimpleTimer <EOL> import numpy as np <EOL> class StateListener ( object ) : <EOL> def __init__ ( self ) : <EOL> self . subscriber = None <EOL> self . transforms = [ ] <EOL> self . timer = SimpleTimer ( ) <EOL> self . paused = True <EOL> def onBDIPose ( self , m ) : <EOL> t = transformUtils . transformFromPose ( m . pos , m . orientation ) <EOL> if self . timer . elapsed ( ) > <NUM_LIT:1.0> : <EOL> self . transforms . append ( ( self . timer . now ( ) , t ) ) <EOL> self . timer . reset ( ) <EOL> def describe ( self ) : <EOL> print '<STR_LIT>' <EOL> print '<STR_LIT>' % len ( self . transforms ) <EOL> if not self . transforms : <EOL> return <EOL> time0 , transform0 = self . transforms [ <NUM_LIT:0> ] <EOL> o0 = np . array ( transform0 . GetOrientation ( ) ) <EOL> p0 = np . array ( transform0 . GetPosition ( ) ) <EOL> for timeN , transformN in self . transforms : <EOL> oN = np . array ( transformN . GetOrientation ( ) ) <EOL> pN = np . array ( transformN . GetPosition ( ) ) <EOL> oD = oN - o0 <EOL> pD = pN - p0 <EOL> print '<STR_LIT>' % ( ( timeN - time0 ) , oD [ <NUM_LIT:0> ] , oD [ <NUM_LIT:1> ] , oD [ <NUM_LIT:2> ] , pD [ <NUM_LIT:0> ] , pD [ <NUM_LIT:1> ] , pD [ <NUM_LIT:2> ] ) <EOL> def init ( self ) : <EOL> self . subscriber = lcmUtils . addSubscriber ( '<STR_LIT>' , lcmbotcore . pose_t , self . onBDIPose ) <EOL> listener = StateListener ( ) <EOL> listener . init ( ) </s>
