return numpy array from tracker, port urrtmon to python3

make_deb.py

@@ -6,7 +6,7 @@ hackish file to crreate deb from setup.py
 import subprocess
 from email.utils import formatdate
 
-DEBVERSION = "0.5"
+DEBVERSION = "0.6"
 
 branch = subprocess.check_output("git rev-parse --abbrev-ref HEAD", shell=True)
 branch = branch.decode()

tools/get_rob.py

@@ -1,12 +1,18 @@
 #!/usr/bin/env python
+import sys
+import logging
+from math import pi
+
 from urx import Robot
 import math3d
-from math import pi
-import logging
 
 if __name__ == "__main__":
+    if len(sys.argv) > 1:
+        host = sys.argv[1]
+    else:
+        host = 'localhost'
     try:
-        robot = Robot( 'localhost')#, logLevel=logging.DEBUG, parserLogLevel=logging.DEBUG)
+        robot = Robot( host )#, logLevel=logging.DEBUG, parserLogLevel=logging.DEBUG)
         r = robot
         from IPython.frontend.terminal.embed import InteractiveShellEmbed
         ipshell = InteractiveShellEmbed( banner1="\nStarting IPython shell, robot object is available\n")

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):
+        result = np.zeros(len(self._data), dtype=[ ('timestamp', 'float64') , ('pose', '6float64'), ('joints', '6float64') ])
+        for idx, data in enumerate(self._data):
             if MATH3D:
-            for data in self._data:
-                data["transform"] = self.inverse * math3d.Transform(data["tcp"])
-        self._queue.put(self._data)
+                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()
 
 

urx/urrtmon.py

@@ -34,7 +34,7 @@ class URRTMonitor(threading.Thread):
     def __init__(self, urHost, debug=False):
         threading.Thread.__init__(self)
         self.daemon = True
-        self._stop = True
+        self._stop_event = True
         self._debug = debug
         self._dataEvent = threading.Condition()
         self._dataAccess = threading.Lock()
@@ -53,7 +53,7 @@ class URRTMonitor(threading.Thread):
         the robot connector socket.'''
         ## Record the time of arrival of the first of the stream block
         recvTime = 0
-        pkg = ''
+        pkg = b''
         while len(pkg) < nBytes:
             pkg += self._rtSock.recv(nBytes - len(pkg))
             if recvTime == 0:
@@ -101,8 +101,7 @@ class URRTMonitor(threading.Thread):
     getTCFVec = tcf_vec
     
     def tcf_pose(self, wait=False, timestamp=False):
-        """ Compute the tool pose *Transform* based on the actual joint
-        values."""
+        """ Return the tool pose values."""
         if wait:
             self.wait()
         with self._dataAccess:
@@ -154,9 +153,7 @@ class URRTMonitor(threading.Thread):
 
     def get_all_data(self, wait=True):
         """
-        return all data parsed from robot as dict
-        The content of the dict may vary depending on version of parser and robot controller
-        but their name should stay constant
+        return all data parsed from robot as a dict
         """
         if wait:
             self.wait()
@@ -165,16 +162,16 @@ class URRTMonitor(threading.Thread):
 
     def stop(self):
         #print(self.__class__.__name__+': Stopping')
-        self._stop = True
+        self._stop_event = True
 
     def cleanup(self):
         self.stop()
         self.join()
 
     def run(self):
-        self._stop = False
+        self._stop_event = False
         self._rtSock.connect((self._urHost, 30003))
-        while not self._stop:
+        while not self._stop_event:
             self.__recv_rt_data()
         self._rtSock.close()
 

urx/ursecmon.py

@@ -294,7 +294,7 @@ class SecondaryMonitor(Thread):
         """
         tstamp = self.lastpacket_timestamp
         with self._dataEvent:
-            self._dataEvent.wait(timeout)#If we haven't received data after 0.5s there is something very wrong
+            self._dataEvent.wait(timeout)
             if tstamp == self.lastpacket_timestamp:
                 raise TimeoutException("Did not receive a valid data packet from robot in {}".format(timeout) )