return numpy array from tracker, port urrtmon to python3

urx/tracker.py

@@ -1,6 +1,8 @@
 import time
 from multiprocessing import Process, Queue, Event
 
+import numpy as np
+
 from urx import urrtmon
 
 MATH3D = True
@@ -15,7 +17,7 @@ class Tracker(Process):
         self.host = robot_host
         self._queue = Queue()
         Process.__init__(self, args=(self._queue,))
-        self._stop = Event()
+        self._stop_event = Event()
         self._finished = Event()
         self._data = []
         if MATH3D:
@@ -31,51 +33,50 @@ class Tracker(Process):
             self.inverse = self.calibration.inverse()
 
     def _save_data(self):
-        if MATH3D:
-            for data in self._data:
-                data["transform"] = self.inverse * math3d.Transform(data["tcp"])
-        self._queue.put(self._data)
+        result = np.zeros(len(self._data), dtype=[ ('timestamp', 'float64') , ('pose', '6float64'), ('joints', '6float64') ])
+        for idx, data in enumerate(self._data):
+            if MATH3D:
+                trf = self.inverse * math3d.Transform(data[1])
+            else:
+                trf = data[1]
+            result[idx] = (data[0], trf.pose_vector, data[2] )
+        self._queue.put(result)
 
 
     def run(self):
-        self._log("Running")
+        self._data = []
         rtmon = urrtmon.URRTMonitor(self.host)
         rtmon.start()
-        while not self._stop.is_set():
-            data = rtmon.get_all_data(wait=True)
-            self._data.append(data)
+        while not self._stop_event.is_set():
+            timestamp, pose = rtmon.tcf_pose(wait=True, timestamp=True)
+            joints = rtmon.q_actual(wait=False, timestamp=False)
+            self._data.append((timestamp, pose, joints))
         self._save_data()
         self._finished.set()
-        self._log("Closing")
 
     def start_acquisition(self):
         self.start()
 
     def stop_acquisition(self):
-        self._stop.set()
+        self._stop_event.set()
+    stop = stop_acquisition
 
     def get_result(self):
-        self._stop.set()
+        self._stop_event.set()
         while not self._finished.is_set():
             time.sleep(0.01)
         return self._queue.get()
 
-    def shutdown(self, join=True):
-        self._stop.set() # just to make sure 
-        self._log("Shutting down")
-        if join:
-            self.join()
-
 
 if __name__ == "__main__":
     p = Tracker()
     try:
-        p.start_acquisition()
+        p.start()
         time.sleep(3)
-        p.stop_acquisition()
+        p.stop()
         a = p.get_result()
         print("Result is: ", a)
     finally:
-        p.shutdown()
+        p.stop()