from ddapp import lcmUtils
from ddapp import transformUtils
import PythonQt
import bot_core as lcmbotcore

from ddapp.simpletimer import SimpleTimer

class StateListener(object):

    def __init__(self):
        self.subscriber = None
        self.transforms = []
        self.timer = SimpleTimer()
        self.paused = True



    def onBDIPose(self, messageData):
        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())
            print '%.2f:' % (timeN - time0), oN - o0, pN - p0

    def init(self):
        self.subscriber = lcmUtils.addSubscriber('POSE_BDI', lcmbotcore.pose_t, self.onBDIPose)


listener = StateListener()
listener.init()

@@ -99,17 +99,16 @@
 botcore%0A
-%0A
 from dda
@@ -142,16 +142,36 @@
 eTimer%0A%0A
+import numpy as np%0A%0A
 class St
@@ -370,18 +370,8 @@
 f, m
-essageData
 ):%0A 
@@ -1042,24 +1042,116 @@
     
-print '%25.2f:
+oD = oN - o0%0A            pD = pN - p0%0A            print '%25.2f: %5B%25.3f, %25.3f, %25.3f%5D  %5B%25.3f, %25.3f, %25.3f%5D  
 ' %25 
+(
 (tim
@@ -1168,23 +1168,48 @@
 ), o
-N - o0, pN - p0
+D%5B0%5D, oD%5B1%5D, oD%5B2%5D, pD%5B0%5D, pD%5B1%5D, pD%5B2%5D)
 %0A%0A  
@@ -1282,18 +1282,19 @@
 ('POSE_B
-DI
+ODY
 ', lcmbo

fix typos in statelistener.py