Coverage for /home/travis/build/RobotLocomotion/director/build/src/director-build/lib/python2.7/dist-packages/director/statelistener : 0%

from director import lcmUtils 

from director import transformUtils 

import PythonQt 

import bot_core as lcmbotcore 

from director.simpletimer import SimpleTimer 

import numpy as np 

class StateListener(object): 

    def __init__(self): 

        self.subscriber = None 

        self.transforms = [] 

        self.timer = SimpleTimer() 

        self.paused = True 

    def onBDIPose(self, m): 

        t = transformUtils.transformFromPose(m.pos, m.orientation) 

        if self.timer.elapsed() > 1.0: 

            self.transforms.append((self.timer.now(), t)) 

            self.timer.reset() 

    def describe(self): 

        print '----------' 

        print '%d transforms' % len(self.transforms) 

        if not self.transforms: 

            return 

        time0, transform0 = self.transforms[0] 

        o0 = np.array(transform0.GetOrientation()) 

        p0 = np.array(transform0.GetPosition()) 

        for timeN, transformN in self.transforms: 

            oN = np.array(transformN.GetOrientation()) 

            pN = np.array(transformN.GetPosition()) 

            oD = oN - o0 

            pD = pN - p0 

            print '%.2f: [%.3f, %.3f, %.3f]  [%.3f, %.3f, %.3f]  ' % ((timeN - time0), oD[0], oD[1], oD[2], pD[0], pD[1], pD[2]) 

    def init(self): 

        self.subscriber = lcmUtils.addSubscriber('POSE_BODY', lcmbotcore.pose_t, self.onBDIPose) 

listener = StateListener() 

listener.init()