diff --git a/urx/tracker.py b/urx/tracker.py index 3477c67..7e2be18 100644 --- a/urx/tracker.py +++ b/urx/tracker.py @@ -40,27 +40,44 @@ 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: - trf = self.inverse * math3d.Transform(data[1]) - else: - trf = data[1] - result[idx] = (data[0], trf.pose_vector, data[2] ) + 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(): - timestamp, pose = rtmon.tcf_pose(wait=True, timestamp=True) - joints = rtmon.q_actual(wait=False, timestamp=False) - self._data.append((timestamp, pose, joints)) + 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() @@ -72,7 +89,7 @@ class Tracker(Process): self._stop_event.set() while not self._finished.is_set(): time.sleep(0.01) - return self._queue.get() + return self._queue.get().copy() if __name__ == "__main__": diff --git a/urx/urrtmon.py b/urx/urrtmon.py index 0f24f10..ef0d1b4 100644 --- a/urx/urrtmon.py +++ b/urx/urrtmon.py @@ -12,7 +12,7 @@ __license__ = "GPLv3" -import os +import logging import socket import struct import time @@ -20,6 +20,8 @@ import threading import numpy as np +import math3d as m3d + class URRTMonitor(threading.Thread): ## Struct for revision of the UR controller giving 692 bytes @@ -29,22 +31,36 @@ class URRTMonitor(threading.Thread): ## pose is not included! rtstruct540 = struct.Struct('>d6d6d6d6d6d6d6d6d18d') - def __init__(self, urHost, debug=False): + def __init__(self, urHost, loglevel=logging.WARNING): threading.Thread.__init__(self) + self.logger = logging.getLogger(self.__class__.__name__) + if len(logging.root.handlers) == 0: #dirty hack + logging.basicConfig() + self.logger.setLevel(loglevel) self.daemon = True self._stop_event = True - self._debug = debug self._dataEvent = threading.Condition() self._dataAccess = threading.Lock() self._rtSock = socket.socket(socket.AF_INET, socket.SOCK_STREAM) + self._rtSock.setsockopt(socket.IPPROTO_TCP, socket.TCP_NODELAY, 1) self._urHost = urHost ## Package data variables self._timestamp = None + self._ctrlTimestamp = None self._qActual = None self._qTarget = None self._tcp = None self._tcp_force = None self.__recvTime = 0 + self._last_ts = 0 + self._buffering = False + self._buffer_lock = threading.Lock() + self._buffer = [] + self._buffer_size_max = 100 + self._current_csys = None + + def set_current_csys(self, csys): + self._current_csys = csys def __recv_bytes(self, nBytes): ''' Facility method for receiving exactly "nBytes" bytes from @@ -85,27 +101,19 @@ class URRTMonitor(threading.Thread): return self._qTarget getTarget = q_target - def tcf_vec(self, wait=False, timestamp=False): - """ Compute the tool pose *6D-vector* based on the actual joint - values.""" - tcfvec = np.zeros(6) - tcf = self.tcf_pose(wait=wait, timestamp=False) - tcfvec[:3] = tcf.pos.data - tcfvec[3:] = tcf.orient.toRotationVector().data - if timestamp: - return self._timestamp, tcfvec - else: - return tcfvec - getTCFVec = tcf_vec - - def tcf_pose(self, wait=False, timestamp=False): + def tcf_pose(self, wait=False, timestamp=False, ctrlTimestamp=False): """ Return the tool pose values.""" if wait: self.wait() with self._dataAccess: tcf = self._tcp - if timestamp: - return self._timestamp, tcf + if ctrlTimestamp or timestamp: + ret = [tcf] + if timestamp: + ret.insert(-1, self._timestamp) + if ctrlTimestamp: + ret.insert(-1, self._ctrlTimestamp) + return ret else: return tcf getTCF = tcf_pose @@ -129,26 +137,55 @@ class URRTMonitor(threading.Thread): ## Record the timestamp for this logical package timestamp = self.__recvTime pkgsize = struct.unpack('>i', head)[0] - if self._debug: - print('Received header telling that package is %d bytes long'%pkgsize) + self.logger.debug('Received header telling that package is %d bytes long'%pkgsize) payload = self.__recv_bytes(pkgsize-4) if pkgsize >= 692: unp = self.rtstruct692.unpack(payload[:self.rtstruct692.size]) elif pkgsize >= 540: unp = self.rtstruct540.unpack(payload[:self.rtstruct540.size]) else: - print('Error, Received packet of length smaller than 540: ', pkgsize) + self.logger.warning('Error, Received packet of length smaller than 540: ', pkgsize) return with self._dataAccess: self._timestamp = timestamp + self._ctrlTimestamp = np.array(unp[0]) + if (self._ctrlTimestamp - self._last_ts) > 0.010: + self.logger.warning("Error the controller failed to send us a packet", self._ctrlTimestamp - self._last_ts) + self._last_ts = self._ctrlTimestamp self._qActual = np.array(unp[31:37]) self._qTarget = np.array(unp[1:7]) self._tcp_force = np.array(unp[67:73]) self._tcp = np.array(unp[73:79]) + + if self._current_csys: + tcp = self._current_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): + self._buffer = [] + self._buffering = True + self._buffer_size_max = maxsize + + def stop_buffering(self): + self._buffering = False + + def pop_buffer(self): + with self._buffer_lock: + if len(self._buffer) > 0: + return self._buffer.pop(0) + else: + return None + def get_all_data(self, wait=True): """ return all data parsed from robot as a dict @@ -156,7 +193,7 @@ class URRTMonitor(threading.Thread): if wait: self.wait() with self._dataAccess: - return dict(timestamp=self._timestamp, qActual=self._qActual, qTarget=self._qTarget, tcp=self._tcp, tcp_force=self._tcp_force) + return dict(timestamp=self._timestamp, ctrltimestamp=self._ctrlTimestamp, qActual=self._qActual, qTarget=self._qTarget, tcp=self._tcp, tcp_force=self._tcp_force) def stop(self): #print(self.__class__.__name__+': Stopping') @@ -177,8 +214,8 @@ class URRTMonitor(threading.Thread): def startupInteractive(): - global urRTMon from optparse import OptionParser + from IPython import embed ## Require the urhost arg. parser = OptionParser() parser.add_option('--debug', action='store_true', default=False, dest='debug') @@ -187,20 +224,16 @@ def startupInteractive(): if len(args) != 1: raise Exception('Must have argument with ur-host name or ip!') urHost = args[0] - print('Connecting to UR Secondary socket inteface on "%s"'%urHost) + print('Connecting to UR real-time socket inteface on "%s"'%urHost) # # Start the connectors urRTMon = URRTMonitor(urHost, debug=opts.debug) - if opts.start: - urRTMon.start() # # Register for hard shutdown - import atexit - atexit.register(urRTMon.stop) - ## Drop to interactive - pystartfile = os.path.expanduser('~/.pythonstartup') - if os.path.isfile(pystartfile): - execfile(pystartfile) - import code - code.interact(None, None, globals()) + try: + if opts.start: + urRTMon.start() + embed() + finally: + urRTMon.stop() if __name__ == '__main__': startupInteractive()