From 75d3184637f4c1bc30d545b55e7ce2147371b8e7 Mon Sep 17 00:00:00 2001 From: Olivier R-D Date: Sun, 25 Aug 2013 16:15:42 +0200 Subject: [PATCH] removed tracker and moved functionnality in urrtmon --- urx/__init__.py | 1 - urx/tracker.py | 106 ------------------------------------------------ urx/urrtmon.py | 34 +++++++++++----- urx/urx.py | 38 ++++++++--------- 4 files changed, 42 insertions(+), 137 deletions(-) delete mode 100644 urx/tracker.py diff --git a/urx/__init__.py b/urx/__init__.py index 303a273..b1180a1 100644 --- a/urx/__init__.py +++ b/urx/__init__.py @@ -6,5 +6,4 @@ __version__ = "0.8" from .urx import Robot, RobotException, URRobot -from .tracker import Tracker diff --git a/urx/tracker.py b/urx/tracker.py deleted file mode 100644 index 7e2be18..0000000 --- a/urx/tracker.py +++ /dev/null @@ -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() - - diff --git a/urx/urrtmon.py b/urx/urrtmon.py index ef0d1b4..e5279aa 100644 --- a/urx/urrtmon.py +++ b/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._current_csys = None + self._csys = None + self._csys_lock = threading.Lock() - def set_current_csys(self, csys): - self._current_csys = csys + def set_csys(self, 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: - tcp = self._current_csys * m3d.Transform(self._tcp)#might be a godd idea to remove dependancy on m3d + if self._csys: + 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 diff --git a/urx/urx.py b/urx/urx.py index 6de861d..b02776a 100644 --- a/urx/urx.py +++ b/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 = urrtmon.URRTMonitor(self.host)# som information is only available on rt interface - else: - self.rtmon = None + self.rtmon = self.get_realtime_monitor() #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