From 7070145c9b3b6db8887de8b7db6e2099ed82c29e Mon Sep 17 00:00:00 2001 From: Olivier R-D Date: Mon, 29 Apr 2013 14:01:20 +0200 Subject: [PATCH] return numpy array from tracker, port urrtmon to python3 --- make_deb.py | 2 +- tools/get_rob.py | 12 +++++++++--- urx/tracker.py | 43 ++++++++++++++++++++++--------------------- urx/urrtmon.py | 17 +++++++---------- urx/ursecmon.py | 2 +- 5 files changed, 40 insertions(+), 36 deletions(-) diff --git a/make_deb.py b/make_deb.py index dff5ee3..5949b58 100755 --- a/make_deb.py +++ b/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() diff --git a/tools/get_rob.py b/tools/get_rob.py index bc136a9..2d3be32 100644 --- a/tools/get_rob.py +++ b/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") diff --git a/urx/tracker.py b/urx/tracker.py index e71168b..fd8f5ea 100644 --- a/urx/tracker.py +++ b/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() diff --git a/urx/urrtmon.py b/urx/urrtmon.py index e28e8d5..7f022cd 100644 --- a/urx/urrtmon.py +++ b/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() diff --git a/urx/ursecmon.py b/urx/ursecmon.py index 366b557..f6d38ae 100644 --- a/urx/ursecmon.py +++ b/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) )