diff --git a/urx/__init__.py b/urx/__init__.py index 29469d9..8b54f9c 100644 --- a/urx/__init__.py +++ b/urx/__init__.py @@ -4,6 +4,4 @@ Python library to control an UR robot through its TCP/IP interface __version__ = "0.9.0" - from urx.robot import Robot, RobotException, URRobot - diff --git a/urx/robot.py b/urx/robot.py index c111563..da1bd40 100644 --- a/urx/robot.py +++ b/urx/robot.py @@ -19,7 +19,7 @@ except ImportError: MATH3D = False print("python-math3d library could not be found on this computer, disabling use of matrices and path blending") -from urx import urrtmon +from urx import urrtmon from urx import ursecmon @@ -28,6 +28,7 @@ class RobotException(Exception): class URRobot(object): + """ Python interface to socket interface of UR robot. programs are send to port 3002 @@ -36,24 +37,24 @@ class URRobot(object): The RT interfaces is only used for the get_force related methods Rmq: A program sent to the robot i executed immendiatly and any running program is stopped """ + def __init__(self, host, useRTInterface=False): self.logger = logging.getLogger(self.__class__.__name__) self.host = host - self.csys = None - + self.csys = None + self.logger.info("Opening secondary monitor socket") - self.secmon = ursecmon.SecondaryMonitor(self.host) #data from robot at 10Hz - + self.secmon = ursecmon.SecondaryMonitor(self.host) # data from robot at 10Hz + self.rtmon = None if useRTInterface: 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 + # 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 # It seems URScript is limited in the character length of floats it accepts - self.max_float_length = 6 # FIXME: check max length!!! + self.max_float_length = 6 # FIXME: check max length!!! - self.secmon.wait() # make sure we get data from robot before letting clients access our methods - + self.secmon.wait() # make sure we get data from robot before letting clients access our methods def __repr__(self): return "Robot Object (IP=%s, state=%s)" % (self.host, self.secmon.get_all_data()["RobotModeData"]) @@ -61,7 +62,7 @@ class URRobot(object): def __str__(self): return self.__repr__() - def is_running(self): + def is_running(self): return self.secmon.running def is_program_running(self): @@ -117,21 +118,21 @@ class URRobot(object): cog.insert(0, weight) prog = "set_payload({}, ({},{},{}))".format(*cog) else: - prog = "set_payload(%s)" % weight + prog = "set_payload(%s)" % weight self.send_program(prog) def set_gravity(self, vector): """ set direction of gravity """ - prog = "set_gravity(%s)" % list(vector) + prog = "set_gravity(%s)" % list(vector) self.send_program(prog) def send_message(self, msg): """ send message to the GUI log tab on the robot controller """ - prog = "textmsg(%s)" % msg + prog = "textmsg(%s)" % msg self.send_program(prog) def set_digital_out(self, output, val): @@ -146,13 +147,13 @@ class URRobot(object): def get_analog_inputs(self): """ - get analog input + get analog input """ return self.secmon.get_analog_inputs() def get_analog_in(self, nb, wait=False): """ - get analog input + get analog input """ return self.secmon.get_analog_in(nb, wait=wait) @@ -178,14 +179,14 @@ class URRobot(object): """ set analog output, val is a float """ - prog = "set_analog_output(%s, %s)" % (output, val) + prog = "set_analog_output(%s, %s)" % (output, val) self.send_program(prog) def set_tool_voltage(self, val): """ set voltage to be delivered to the tool, val is 0, 12 or 24 """ - prog = "set_tool_voltage(%s)" % (val) + prog = "set_tool_voltage(%s)" % (val) self.send_program(prog) def wait_for_move(self, radius=0, target=None): @@ -194,7 +195,7 @@ class URRobot(object): radius and target args are ignored """ self.logger.debug("Waiting for move completion") - # it is necessary to wait since robot may takes a while to get into running state, + # it is necessary to wait since robot may takes a while to get into running state, for _ in range(3): self.secmon.wait() while True: @@ -203,9 +204,9 @@ class URRobot(object): jts = self.secmon.get_joint_data(wait=True) finished = True 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: - self.logger.debug("Waiting for end move, q_actual is %s, q_target is %s, diff is %s, epsilon is %s", jts["q_actual%s"%i], jts["q_target%s"%i], jts["q_actual%s"%i] - jts["q_target%s"%i], self.joinEpsilon) + # 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: + self.logger.debug("Waiting for end move, q_actual is %s, q_target is %s, diff is %s, epsilon is %s", 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(): @@ -246,7 +247,7 @@ class URRobot(object): if relative: l = self.getj() joints = [v + l[i] for i, v in enumerate(joints)] - joints = [round(j, self.max_float_length) for j in joints] + joints = [round(j, self.max_float_length) for j in joints] joints.append(acc) joints.append(vel) joints.append(radius) @@ -260,7 +261,7 @@ class URRobot(object): def movel(self, tpose, acc=0.01, vel=0.01, radius=0, wait=True, relative=False): """ - linear move + linear move """ if relative: l = self.getl() @@ -328,7 +329,7 @@ class URRobot(object): def movels(self, pose_list, acc=0.01, vel=0.01, radius=0.01, wait=True): """ Concatenate several movel commands and applies a blending radius - pose_list is a list of pose. + pose_list is a list of pose. """ header = "def myProg():\n" end = "end\n" @@ -337,7 +338,7 @@ class URRobot(object): for idx, pose in enumerate(pose_list): pose.append(acc) pose.append(vel) - if idx != (len(pose_list) -1): + if idx != (len(pose_list) - 1): pose.append(radius) else: pose.append(0) @@ -367,11 +368,11 @@ 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 hardware 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): """ - set robot in freedrive/brackdrive mode where an operator can jogg + set robot in freedrive/brackdrive mode where an operator can jogg the robot to wished pose """ if val: @@ -392,33 +393,33 @@ class URRobot(object): """ 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 = urrtmon.URRTMonitor(self.host) # som information is only available on rt interface self.rtmon.start() - self.rtmon.set_csys(self.csys) + self.rtmon.set_csys(self.csys) return self.rtmon - - class Robot(URRobot): + """ Generic Python interface to an industrial robot. Compared to the URRobot class, this class adds the possibilty to work directly with matrices and includes support for calibrating the robot coordinate system """ + def __init__(self, host, useRTInterface=False): URRobot.__init__(self, host, useRTInterface) self.default_linear_acceleration = 0.01 self.default_linear_velocity = 0.01 self.csys_dict = {} self.csys = None - self.set_csys("Robot", m3d.Transform()) #identity + self.set_csys("Robot", m3d.Transform()) # identity def set_tcp(self, tcp): """ set robot flange to tool tip transformation """ - if type(tcp) == m3d.Transform: + if isinstance(tcp, m3d.Transform): tcp = tcp.pose_vector URRobot.set_tcp(self, tcp) @@ -442,7 +443,7 @@ class Robot(URRobot): set tool orientation using a orientation matric from math3d or a orientation vector """ - if type(orient) is not m3d.Orientation: + if not isinstance(orient, m3d.Orientation): orient = m3d.Orientation(orient) trans = self.get_pose() trans.orient = orient @@ -453,7 +454,7 @@ class Robot(URRobot): move tool in base coordinate, keeping orientation """ t = m3d.Transform() - if not type(vect) is m3d.Vector: + if not isinstance(vect, m3d.Vector): vect = m3d.Vector(vect) t.pos += m3d.Vector(vect) return self.add_pose_base(t, acc, vel, radius, wait=wait) @@ -463,7 +464,7 @@ class Robot(URRobot): move tool in tool coordinate, keeping orientation """ t = m3d.Transform() - if not type(vect) is m3d.Vector: + if not isinstance(vect, m3d.Vector): vect = m3d.Vector(vect) t.pos += vect return self.add_pose_tool(t, acc, vel, radius, wait=wait) @@ -472,7 +473,7 @@ class Robot(URRobot): """ set tool to given pos, keeping constant orientation """ - if not type(vect) is m3d.Vector: + if not isinstance(vect, m3d.Vector): vect = m3d.Vector(vect) trans = m3d.Transform(self.get_orientation(), m3d.Vector(vect)) return self.set_pose(trans, acc, vel, radius, wait=wait) @@ -482,22 +483,21 @@ class Robot(URRobot): move tcp to point and orientation defined by a transformation if process is True, movep is used instead of movel if radius is not 0 and wait is True, the method will return when tcp - is radius close to the target. It is then possible to send another command + is radius close to the target. It is then possible to send another command and the controller will blend the path. Blending only works with process(movep). (BROKEN!) """ - if not acc: + if not acc: acc = self.default_linear_acceleration - if not vel: + if not vel: vel = self.default_linear_velocity t = self.csys * trans if process: pose = URRobot.movep(self, t.pose_vector, acc=acc, vel=vel, wait=wait, radius=radius) else: pose = URRobot.movel(self, t.pose_vector, acc=acc, vel=vel, wait=wait, radius=radius) - if pose != None: #movel does not return anything when wait is False + if pose is not None: # movel does not return anything when wait is False return self.csys.inverse * m3d.Transform(pose) - def add_pose_base(self, trans, acc=None, vel=None, radius=0, wait=True, process=False): """ Add transform expressed in base coordinate @@ -517,7 +517,7 @@ class Robot(URRobot): get current transform from base to to tcp """ pose = URRobot.getl(self, wait) - trans = self.csys.inverse * m3d.Transform(pose) + trans = self.csys.inverse * m3d.Transform(pose) return trans def get_orientation(self, wait=False): @@ -565,7 +565,7 @@ class Robot(URRobot): def movels(self, pose_list, acc=0.01, vel=0.01, radius=0.01, wait=True): """ Concatenate several movep commands and applies a blending radius - pose_list is a list of pose. + pose_list is a list of pose. """ new_poses = [] for pose in pose_list: @@ -606,7 +606,7 @@ class Robot(URRobot): return t.pose_vector.tolist() def set_gravity(self, vector): - if type(vector) == m3d.Vector: + if isinstance(vector, m3d.Vector): vector = vector.list return URRobot.set_gravity(self, vector) @@ -616,7 +616,7 @@ class Robot(URRobot): if radius is not 0, returns when tcp is radius closed to target(using mathd3d dist method) """ self.logger.debug("Waiting for move completion") - # it is necessary to wait since robot may takes a while to get into running state, + # it is necessary to wait since robot may takes a while to get into running state, for _ in range(3): self.secmon.wait() target = m3d.Transform(target) @@ -631,12 +631,5 @@ class Robot(URRobot): return - - - if not MATH3D: - Robot = URRobot - - - - + Robot = URRobot diff --git a/urx/urrtmon.py b/urx/urrtmon.py index c02890f..5441084 100644 --- a/urx/urrtmon.py +++ b/urx/urrtmon.py @@ -11,10 +11,9 @@ __credits__ = ["Morten Lind, Olivier Roulet-Dubonnet"] __license__ = "GPLv3" - import logging import socket -import struct +import struct import time import threading from copy import deepcopy @@ -23,19 +22,20 @@ import numpy as np import math3d as m3d + class URRTMonitor(threading.Thread): - ## Struct for revision of the UR controller giving 692 bytes + # Struct for revision of the UR controller giving 692 bytes rtstruct692 = struct.Struct('>d6d6d6d6d6d6d6d6d18d6d6d6dQ') - - ## for revision of the UR controller giving 540 byte. Here TCP - ## pose is not included! + + # for revision of the UR controller giving 540 byte. Here TCP + # pose is not included! rtstruct540 = struct.Struct('>d6d6d6d6d6d6d6d6d18d') 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 + if len(logging.root.handlers) == 0: # dirty hack logging.basicConfig() self.logger.setLevel(loglevel) self.daemon = True @@ -45,13 +45,13 @@ class URRTMonitor(threading.Thread): 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 + # Package data variables self._timestamp = None self._ctrlTimestamp = None self._qActual = None self._qTarget = None self._tcp = None - self._tcp_force = None + self._tcp_force = None self.__recvTime = 0 self._last_ctrl_ts = 0 #self._last_ts = 0 @@ -68,13 +68,13 @@ class URRTMonitor(threading.Thread): def __recv_bytes(self, nBytes): ''' Facility method for receiving exactly "nBytes" bytes from the robot connector socket.''' - ## Record the time of arrival of the first of the stream block + # Record the time of arrival of the first of the stream block recvTime = 0 pkg = b'' while len(pkg) < nBytes: pkg += self._rtSock.recv(nBytes - len(pkg)) if recvTime == 0: - recvTime = time.time() + recvTime = time.time() self.__recvTime = recvTime return pkg @@ -92,7 +92,7 @@ class URRTMonitor(threading.Thread): else: return self._qActual getActual = q_actual - + def q_target(self, wait=False, timestamp=False): """ Get the target joint position vector.""" if wait: @@ -103,7 +103,7 @@ class URRTMonitor(threading.Thread): else: return self._qTarget getTarget = q_target - + def tcf_pose(self, wait=False, timestamp=False, ctrlTimestamp=False): """ Return the tool pose values.""" if wait: @@ -137,28 +137,37 @@ class URRTMonitor(threading.Thread): def __recv_rt_data(self): head = self.__recv_bytes(4) - ## Record the timestamp for this logical package + # Record the timestamp for this logical package timestamp = self.__recvTime pkgsize = struct.unpack('>i', head)[0] - self.logger.debug('Received header telling that package is %d bytes long'%pkgsize) - payload = self.__recv_bytes(pkgsize-4) + 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: - self.logger.warning('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 # it seems that packet often arrives packed as two... maybe TCP_NODELAY is not set on UR controller?? - #if (self._timestamp - self._last_ts) > 0.010: - #self.logger.warning("Error the we did not receive a packet for {}s ".format( self._timestamp - self._last_ts)) + # if (self._timestamp - self._last_ts) > 0.010: + #self.logger.warning("Error the we did not receive a packet for {}s ".format( self._timestamp - self._last_ts)) #self._last_ts = self._timestamp - self._ctrlTimestamp = np.array(unp[0]) - if self._last_ctrl_ts != 0 and (self._ctrlTimestamp - self._last_ctrl_ts) > 0.010: - self.logger.warning("Error the controller failed to send us a packet: time since last packet {}s ".format( self._ctrlTimestamp - self._last_ctrl_ts)) + self._ctrlTimestamp = np.array(unp[0]) + if self._last_ctrl_ts != 0 and ( + self._ctrlTimestamp - + self._last_ctrl_ts) > 0.010: + self.logger.warning( + "Error the controller failed to send us a packet: time since last packet {}s ".format( + self._ctrlTimestamp - + self._last_ctrl_ts)) self._last_ctrl_ts = self._ctrlTimestamp self._qActual = np.array(unp[31:37]) self._qTarget = np.array(unp[1:7]) @@ -167,11 +176,16 @@ class URRTMonitor(threading.Thread): if self._csys: with self._csys_lock: - tcp = self._csys * m3d.Transform(self._tcp)#might be a godd idea to remove dependancy on m3d + # might be a godd idea to remove dependancy on m3d + tcp = self._csys * m3d.Transform(self._tcp) 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 + self._buffer.append( + (self._timestamp, + self._ctrlTimestamp, + self._tcp, + self._qActual)) # FIXME use named arrays of allow to configure what data to buffer with self._dataEvent: self._dataEvent.notifyAll() @@ -206,7 +220,6 @@ class URRTMonitor(threading.Thread): return self._buffer.pop(0) time.sleep(0.001) - def get_buffer(self): """ return a copy of the entire buffer @@ -221,7 +234,13 @@ class URRTMonitor(threading.Thread): if wait: self.wait() with self._dataAccess: - return dict(timestamp=self._timestamp, ctrltimestamp=self._ctrlTimestamp, 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') @@ -239,20 +258,26 @@ class URRTMonitor(threading.Thread): self._rtSock.close() - - def startupInteractive(): from optparse import OptionParser from IPython import embed - ## Require the urhost arg. + # Require the urhost arg. parser = OptionParser() - parser.add_option('--debug', action='store_true', default=False, dest='debug') - parser.add_option('--start', action='store_true', default=False, dest='start') + parser.add_option( + '--debug', + action='store_true', + default=False, + dest='debug') + parser.add_option( + '--start', + action='store_true', + default=False, + dest='start') opts, args = parser.parse_args() if len(args) != 1: raise Exception('Must have argument with ur-host name or ip!') urHost = args[0] - print('Connecting to UR real-time 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 diff --git a/urx/ursecmon.py b/urx/ursecmon.py index 0127776..57494e0 100644 --- a/urx/ursecmon.py +++ b/urx/ursecmon.py @@ -14,25 +14,26 @@ __license__ = "GPLv3" from threading import Thread, Condition, Lock import logging -import struct +import struct import socket from copy import copy import time - class ParsingException(Exception): + def __init__(self, *args): Exception.__init__(self, *args) + class TimeoutException(Exception): + def __init__(self, *args): Exception.__init__(self, *args) - - class ParserUtils(object): + def __init__(self): self.logger = logging.getLogger(__name__) self.is_v30 = False @@ -42,13 +43,13 @@ class ParserUtils(object): parse a packet from the UR socket and return a dictionary with the data """ allData = {} - #print "Total size ", len(data) + # print "Total size ", len(data) while data: psize, ptype, pdata, data = self.analyze_header(data) - #print "We got packet with size %i and type %s" % (psize, ptype) + # print "We got packet with size %i and type %s" % (psize, ptype) if ptype == 16: allData["SecondaryClientData"] = self._get_data(pdata, "!iB", ("size", "type")) - data = (pdata + data)[5:] # This is the total size so we resend data to parser + data = (pdata + data)[5:] # This is the total size so we resend data to parser elif ptype == 0: # this parses RobotModeData for versions >=3.0 (i.e. 3.0) if psize == 38: @@ -68,14 +69,14 @@ class ParserUtils(object): elif ptype == 5: allData["LaserPointer(OBSOLETE)"] = self._get_data(pdata, "iBddd", ("size", "type")) elif ptype == 3: - allData["MasterBoardData"] = self._get_data(pdata, "iBhhbbddbbddffffBBb", ("size", "type", "digitalInputBits", "digitalOutputBits", "analogInputRange0", "analogInputRange1", "analogInput0", "analogInput1", "analogInputDomain0", "analogInputDomain1", "analogOutput0", "analogOutput1", "masterBoardTemperature", "robotVoltage48V", "robotCurrent", "masterIOCurrent"))#, "masterSafetyState" ,"masterOnOffState", "euromap67InterfaceInstalled" )) + allData["MasterBoardData"] = self._get_data(pdata, "iBhhbbddbbddffffBBb", ("size", "type", "digitalInputBits", "digitalOutputBits", "analogInputRange0", "analogInputRange1", "analogInput0", "analogInput1", "analogInputDomain0", "analogInputDomain1", "analogOutput0", "analogOutput1", "masterBoardTemperature", "robotVoltage48V", "robotCurrent", "masterIOCurrent")) # , "masterSafetyState" ,"masterOnOffState", "euromap67InterfaceInstalled" )) elif ptype == 2: allData["ToolData"] = self._get_data(pdata, "iBbbddfBffB", ("size", "type", "analoginputRange2", "analoginputRange3", "analogInput2", "analogInput3", "toolVoltage48V", "toolOutputVoltage", "toolCurrent", "toolTemperature", "toolMode")) - #elif ptype == 8: - #allData["varMessage"] = self._get_data(pdata, "!iBQbb iiBAcAc", ("size", "type", "timestamp", "source", "robotMessageType", "code", "argument", "titleSize", "messageTitle", "messageText")) - #elif ptype == 7: - #allData["keyMessage"] = self._get_data(pdata, "!iBQbb iiBAcAc", ("size", "type", "timestamp", "source", "robotMessageType", "code", "argument", "titleSize", "messageTitle", "messageText")) + # elif ptype == 8: + #allData["varMessage"] = self._get_data(pdata, "!iBQbb iiBAcAc", ("size", "type", "timestamp", "source", "robotMessageType", "code", "argument", "titleSize", "messageTitle", "messageText")) + # elif ptype == 7: + #allData["keyMessage"] = self._get_data(pdata, "!iBQbb iiBAcAc", ("size", "type", "timestamp", "source", "robotMessageType", "code", "argument", "titleSize", "messageTitle", "messageText")) elif ptype == 20: tmp = self._get_data(pdata, "!iB Qbb", ("size", "type", "timestamp", "source", "robotMessageType")) @@ -110,7 +111,7 @@ class ParserUtils(object): names args are strings used to store values """ tmpdata = copy(data) - fmt = fmt.strip() # space may confuse us + fmt = fmt.strip() # space may confuse us d = dict() i = 0 j = 0 @@ -118,32 +119,32 @@ class ParserUtils(object): f = fmt[j] if f in (" ", "!", ">", "<"): j += 1 - elif f == "A": #we got an array + elif f == "A": # we got an array # first we need to find its size - if j == len(fmt) - 2: # we are last element, size is the rest of data in packet + if j == len(fmt) - 2: # we are last element, size is the rest of data in packet arraysize = len(tmpdata) - else: # size should be given in last element - asn = names[i-1] + else: # size should be given in last element + asn = names[i - 1] if not asn.endswith("Size"): raise ParsingException("Error, array without size ! %s %s" % (asn, i)) else: arraysize = d[asn] d[names[i]] = tmpdata[0:arraysize] - #print "Array is ", names[i], d[names[i]] + # print "Array is ", names[i], d[names[i]] tmpdata = tmpdata[arraysize:] j += 2 i += 1 else: fmtsize = struct.calcsize(fmt[j]) - #print "reading ", f , i, j, fmtsize, len(tmpdata) - if len(tmpdata) < fmtsize: #seems to happen on windows + # print "reading ", f , i, j, fmtsize, len(tmpdata) + if len(tmpdata) < fmtsize: # seems to happen on windows raise ParsingException("Error, length of data smaller than advertized: ", len(tmpdata), fmtsize, "for names ", names, f, i, j) d[names[i]] = struct.unpack("!" + f, tmpdata[0:fmtsize])[0] - #print names[i], d[names[i]] + # print names[i], d[names[i]] tmpdata = tmpdata[fmtsize:] j += 1 i += 1 - return d + return d def get_header(self, data): return struct.unpack("!iB", data[0:5]) @@ -156,10 +157,10 @@ class ParserUtils(object): raise ParsingException("Packet size %s smaller than header size (5 bytes)" % len(data)) else: psize, ptype = self.get_header(data) - if psize < 5: + if psize < 5: raise ParsingException("Error, declared length of data smaller than its own header(5): ", psize) elif psize > len(data): - raise ParsingException("Error, length of data smaller (%s) than declared (%s)" %(len(data), psize)) + raise ParsingException("Error, length of data smaller (%s) than declared (%s)" % (len(data), psize)) return psize, ptype, data[:psize], data[psize:] def find_first_packet(self, data): @@ -183,7 +184,7 @@ class ParserUtils(object): self.logger.debug("Got packet with size %s and type %s", psize, ptype) if counter: self.logger.info("Remove {0} bytes of garbage at begining of packet".format(counter)) - #ok we we have somehting which looks like a packet" + # ok we we have somehting which looks like a packet" return (data[:psize], data[psize:]) else: #packet is not complete @@ -194,11 +195,12 @@ class ParserUtils(object): return None - class SecondaryMonitor(Thread): + """ Monitor data from secondary port and send programs to robot """ + def __init__(self, host): Thread.__init__(self) self.logger = logging.getLogger(self.__class__.__name__) @@ -211,32 +213,31 @@ class SecondaryMonitor(Thread): self._prog_queue = [] self._prog_queue_lock = Lock() self._dataqueue = bytes() - self._trystop = False # to stop thread - self.running = False #True when robot is on and listening + self._trystop = False # to stop thread + self.running = False # True when robot is on and listening self._dataEvent = Condition() self.lastpacket_timestamp = 0 self.start() - self.wait()# make sure we got some data before someone calls us + self.wait() # make sure we got some data before someone calls us def send_program(self, prog): """ send program to robot in URRobot format - If another program is send while a program is running the first program is aborded. + If another program is send while a program is running the first program is aborded. """ prog.strip() self.logger.debug("Sending program: " + prog) - if type(prog) != bytes: + if not isinstance(prog, bytes): prog = prog.encode() with self._prog_queue_lock: self._prog_queue.append(prog + b"\n") - def run(self): """ - check program execution status in the secondary client data packet we get from the robot + check program execution status in the secondary client data packet we get from the robot This interface uses only data from the secondary client interface (see UR doc) - Only the last connected client is the primary client, + Only the last connected client is the primary client, so this is not guaranted and we cannot rely on information to the primary client. """ @@ -245,12 +246,12 @@ class SecondaryMonitor(Thread): if len(self._prog_queue) > 0: prog = self._prog_queue.pop(0) self._s_secondary.send(prog) - + data = self._get_data() try: tmpdict = self._parser.parse(data) with self._dictLock: - self._dict = tmpdict + self._dict = tmpdict except ParsingException as ex: self.logger.warn("Error parsing one packet from urrobot: " + str(ex)) continue @@ -259,26 +260,26 @@ class SecondaryMonitor(Thread): self.logger.warn("Got a packet from robot without RobotModeData, strange ...") continue - self.lastpacket_timestamp = time.time() - + self.lastpacket_timestamp = time.time() + if self._parser.is_v30: if self._dict["RobotModeData"]["robotMode"] == 7 \ - and self._dict["RobotModeData"]["isRealRobotEnabled"] == True \ - and self._dict["RobotModeData"]["isEmergencyStopped"] == False \ - and self._dict["RobotModeData"]["isSecurityStopped"] == False \ - and self._dict["RobotModeData"]["isRobotConnected"] == True \ - and self._dict["RobotModeData"]["isPowerOnRobot"] == True: + and self._dict["RobotModeData"]["isRealRobotEnabled"] == True \ + and self._dict["RobotModeData"]["isEmergencyStopped"] == False \ + and self._dict["RobotModeData"]["isSecurityStopped"] == False \ + and self._dict["RobotModeData"]["isRobotConnected"] == True \ + and self._dict["RobotModeData"]["isPowerOnRobot"] == True: self.running = True else: if self._dict["RobotModeData"]["robotMode"] == 0 \ - and self._dict["RobotModeData"]["isRealRobotEnabled"] == True \ - and self._dict["RobotModeData"]["isEmergencyStopped"] == False \ - and self._dict["RobotModeData"]["isSecurityStopped"] == False \ - and self._dict["RobotModeData"]["isRobotConnected"] == True \ - and self._dict["RobotModeData"]["isPowerOnRobot"] == True: + and self._dict["RobotModeData"]["isRealRobotEnabled"] == True \ + and self._dict["RobotModeData"]["isEmergencyStopped"] == False \ + and self._dict["RobotModeData"]["isSecurityStopped"] == False \ + and self._dict["RobotModeData"]["isRobotConnected"] == True \ + and self._dict["RobotModeData"]["isPowerOnRobot"] == True: self.running = True else: - if self.running == True: + if self.running: self.logger.error("Robot not running: " + str(self._dict["RobotModeData"])) self.running = False with self._dataEvent: @@ -344,7 +345,7 @@ class SecondaryMonitor(Thread): with self._dictLock: output = self._dict["MasterBoardData"]["digitalOutputBits"] mask = 1 << nb - if output & mask: + if output & mask: return 1 else: return 0 @@ -355,7 +356,7 @@ class SecondaryMonitor(Thread): with self._dictLock: output = self._dict["MasterBoardData"]["digitalInputBits"] mask = 1 << nb - if output & mask: + if output & mask: return 1 else: return 0 @@ -388,16 +389,11 @@ class SecondaryMonitor(Thread): with self._dictLock: return self._dict["RobotModeData"]["isProgramRunning"] - def cleanup(self): self._trystop = True self.join() - #with self._dataEvent: #wake up any thread that may be waiting for data before we close. Should we do that? - #self._dataEvent.notifyAll() + # with self._dataEvent: #wake up any thread that may be waiting for data before we close. Should we do that? + # self._dataEvent.notifyAll() if self._s_secondary: with self._prog_queue_lock: self._s_secondary.close() - - - -