removed tracker and moved functionnality in urrtmon

urx/__init__.py

@@ -6,5 +6,4 @@ __version__ = "0.8"
 
 
 from .urx import Robot, RobotException, URRobot
-from .tracker import Tracker
 

urx/tracker.py

@@ -1,106 +0,0 @@
-"""
-tracks moves a UR robot over the UR real-time port
-"""
-__author__ = "Olivier Roulet-Dubonnet"
-__copyright__ = "Copyright 2011-2013, Sintef Raufoss Manufacturing"
-__credits__ = ["Olivier Roulet-Dubonnet"]
-__license__ = "GPLv3"
-import time
-from multiprocessing import Process, Queue, Event
-
-import numpy as np
-
-from urx import urrtmon
-
-MATH3D = True
-try:
-    import math3d
-except ImportError:
-    MATH3D = False
-    print("pymath3d library could not be found on this computer, disabling use of matrices")
-
-class Tracker(Process):
-    def __init__(self, robot_host):
-        self.host = robot_host
-        self._queue = Queue()
-        Process.__init__(self, args=(self._queue,))
-        self._stop_event = Event()
-        self._finished = Event()
-        self._data = []
-        if MATH3D:
-            self.calibration = math3d.Transform()
-            self.inverse = self.calibration.inverse()
-
-    def _log(self, *args):
-        print(self.__class__.__name__, ": ".join([str(i) for i in args]))
-
-    def set_csys(self, cal):
-        if MATH3D:
-            self.calibration = cal
-            self.inverse = self.calibration.inverse()
-
-    def _save_data(self):
-        result = np.array(self._data, dtype=[ ('timestamp', 'float64'), ('ctrlTimestamp', 'float64'), ('pose', '6float64'), ('joints', '6float64') ])
-        if MATH3D:
-            nposes = []
-            for pose in result["pose"]:
-                trf = self.inverse * math3d.Transform(pose) 
-                nposes.append(trf.pose_vector) 
-            result["pose"] = np.array(nposes)
-        self._queue.put(result)
-
-
-    def run_old(self):
-        self._data = []
-        rtmon = urrtmon.URRTMonitor(self.host)
-        rtmon.start()
-        while not self._stop_event.is_set():
-            timestamp, ctrlTimestamp, pose = rtmon.tcf_pose(wait=True, timestamp=True, ctrlTimestamp=True)
-            joints = rtmon.q_actual(wait=False, timestamp=False)
-            self._data.append((timestamp, ctrlTimestamp, pose, joints))
-        self._save_data()
-        self._finished.set()
-
-    def run(self):
-        self._data = []
-        rtmon = urrtmon.URRTMonitor(self.host)
-        rtmon.start()
-        rtmon.start_buffering()
-        while not self._stop_event.is_set():
-            data = rtmon.pop_buffer()
-            if data:
-                self._data.append(data)
-            else:
-                time.sleep(0.002)
-        self._save_data()
-        rtmon.stop_buffering()
-        rtmon.stop()
-        self._finished.set()
-
-
-    def start_acquisition(self):
-        self.start()
-
-    def stop_acquisition(self):
-        self._stop_event.set()
-    stop = stop_acquisition
-
-    def get_result(self):
-        self._stop_event.set()
-        while not self._finished.is_set():
-            time.sleep(0.01)
-        return self._queue.get().copy()
-
-
-if __name__ == "__main__":
-    p = Tracker()
-    try:
-        p.start()
-        time.sleep(3)
-        p.stop()
-        a = p.get_result()
-        print("Result is: ", a)
-    finally:
-        p.stop()
-
-

urx/urrtmon.py

@@ -17,6 +17,7 @@ import socket
 import struct 
 import time
 import threading
+from copy import deepcopy
 
 import numpy as np
 
@@ -56,11 +57,12 @@ class URRTMonitor(threading.Thread):
         self._buffering = False
         self._buffer_lock = threading.Lock()
         self._buffer = []
-        self._buffer_size_max = 100
+        self._csys = None
-        self._current_csys = None
+        self._csys_lock = threading.Lock()
 
-    def set_current_csys(self, csys):
+    def set_csys(self, csys):
-        self._current_csys = csys
+        with self._csys_lock:
+            self._csys = csys
 
     def __recv_bytes(self, nBytes):
         ''' Facility method for receiving exactly "nBytes" bytes from
@@ -158,34 +160,44 @@ class URRTMonitor(threading.Thread):
             self._tcp_force = np.array(unp[67:73])
             self._tcp = np.array(unp[73:79])
 
-            if self._current_csys:
+            if self._csys:
-                tcp = self._current_csys * m3d.Transform(self._tcp)#might be a godd idea to remove dependancy on m3d
+                with self._csys_lock:
+                    tcp = self._csys * m3d.Transform(self._tcp)#might be a godd idea to remove dependancy on m3d
                 self._tcp = tcp.pose_vector
         if self._buffering:
             with self._buffer_lock:
                 self._buffer.append((self._timestamp, self._ctrlTimestamp, self._tcp, self._qActual))#FIXME use named arrays of allow to configure what data to buffer
-                if len(self._buffer) > self._buffer_size_max:
-                    self.logger.warning("Warning: buffer full")
-                    self._buffer.pop()
 
         with self._dataEvent:
             self._dataEvent.notifyAll()
 
-    def start_buffering(self, maxsize=100):
+    def start_buffering(self):
+        """
+        start buffering all data from controller
+        """
         self._buffer = []
         self._buffering = True
-        self._buffer_size_max = maxsize
 
     def stop_buffering(self):
         self._buffering = False
 
     def pop_buffer(self):
+        """
+        return oldest value in buffer
+        """
         with self._buffer_lock:
             if len(self._buffer) > 0:
                 return self._buffer.pop(0)
             else:
                 return None
 
+    def get_buffer(self):
+        """
+        return a copy of the entire buffer
+        """
+        with self._buffer_lock:
+            return deepcopy(self._buffer)
+
     def get_all_data(self, wait=True):
         """
         return all data parsed from robot as a dict

urx/urx.py

@@ -23,7 +23,6 @@ except ImportError:
 
 from urx import urrtmon 
 from urx import ursecmon
-from urx import tracker
 
 
 class RobotException(Exception):
@@ -45,21 +44,18 @@ class URRobot(object):
             logging.basicConfig()
         self.logger.setLevel(logLevel)
         self.host = host
+        self.csys = None 
         
         self.logger.info("Opening secondary monitor socket")
         self.secmon = ursecmon.SecondaryMonitor(self.host, logLevel=logLevel, parserLogLevel=parserLogLevel) #data from robot at 10Hz
-        
+       
+        self.rtmon = None
         if useRTInterface:
-            self.logger.info("Opening real-time monitor socket")
+            self.rtmon = self.get_realtime_monitor()
-            self.rtmon = urrtmon.URRTMonitor(self.host)# som information is only available on rt interface
-        else:
-            self.rtmon = None
         #the next 3 values must be conservative! otherwise we may wait forever
         self.joinEpsilon = 0.01 # precision of joint movem used to wait for move completion
         #URScript is  limited in the character length of floats it accepts
         self.max_float_length = 6 # FIXME: check max length!!!
-        if useRTInterface:
-            self.rtmon.start()
 
         self.secmon.wait() # make sure we get data to not suprise clients
         
@@ -240,7 +236,7 @@ class URRobot(object):
             for i in range(0, 6):
                 #Rmq: q_target is an interpolated target we have no control over
                 if abs(jts["q_actual%s"%i] - jts["q_target%s"%i]) > self.joinEpsilon:
-                    print("Waiting for end move, q_actual is {}, q_target is {}, diff is {}, epsilon is {}".format( jts["q_actual%s"%i], jts["q_target%s"%i]  , jts["q_actual%s"%i] - jts["q_target%s"%i], self.radialEpsilon))
+                    print("Waiting for end move, q_actual is {}, q_target is {}, diff is {}, epsilon is {}".format( jts["q_actual%s"%i], jts["q_target%s"%i]  , jts["q_actual%s"%i] - jts["q_target%s"%i], self.joinEpsilon))
                     finished = False
                     break
             if finished and not self.secmon.is_program_running():
@@ -382,7 +378,7 @@ class URRobot(object):
         self.secmon.cleanup()
         if self.rtmon:
             self.rtmon.stop()
-    shutdown = cleanup #this might be wrong since we could also shutdown the robot from this script
+    shutdown = cleanup #this might be wrong since we could also shutdown the robot hardware from this script
 
     def set_freedrive(self, val):
         if val:
@@ -396,6 +392,19 @@ class URRobot(object):
         else:
             self.send_program("set real")
 
+    def get_realtime_monitor(self):
+        """
+        return a pointer to the realtime monitor object
+        usefull to track robot position for example
+        """
+        if not self.rtmon:
+            self.logger.info("Opening real-time monitor socket")
+            self.rtmon = urrtmon.URRTMonitor(self.host)# som information is only available on rt interface
+            self.rtmon.start()
+        self.rtmon.set_csys(self.csys) 
+        return self.rtmon
+
+
 
 
 class Robot(URRobot):
@@ -412,7 +421,6 @@ class Robot(URRobot):
         self.csys = None
         self.csys_inv = None
         self.set_csys("Robot", m3d.Transform()) #identity
-        self.tracker = None
 
     def set_tcp(self, tcp):
         if type(tcp) == m3d.Transform:
@@ -590,14 +598,6 @@ class Robot(URRobot):
             vector = vector.list
         return URRobot.set_gravity(self, vector)
 
-    def get_tracker(self):
-        """
-        return an object able to track robot move for logging
-        """
-        t = tracker.Tracker(self.host)
-        t.set_csys(self.csys)
-        return t
-
 
 if not MATH3D:
     Robot = URRobot