changes in tracking, to allow for buffering of data
This commit is contained in:
Olivier R-D
parent
ad031f52c7
commit
64726bf804
@ -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):
|
||||
result = np.array(self._data, dtype=[ ('timestamp', 'float64'), ('ctrlTimestamp', 'float64'), ('pose', '6float64'), ('joints', '6float64') ])
|
||||
if MATH3D:
|
||||
trf = self.inverse * math3d.Transform(data[1])
|
||||
else:
|
||||
trf = data[1]
|
||||
result[idx] = (data[0], trf.pose_vector, data[2] )
|
||||
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__":
|
||||
|
||||
@ -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 ctrlTimestamp or timestamp:
|
||||
ret = [tcf]
|
||||
if timestamp:
|
||||
return self._timestamp, tcf
|
||||
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)
|
||||
# # Register for hard shutdown
|
||||
try:
|
||||
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())
|
||||
embed()
|
||||
finally:
|
||||
urRTMon.stop()
|
||||
|
||||
if __name__ == '__main__':
|
||||
startupInteractive()
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user