""" parse data from UR socket client REMARK Only the last connected socket on 3001 is the primary client !!!! so it is unreliable to rely on it http://support.universal-robots.com/Technical/PrimaryAndSecondaryClientInterface """ __author__ = "Olivier Roulet-Dubonnet" __copyright__ = "Copyright 2011-2012, Olivier Roulet-Dubonnet" __credits__ = ["Olivier Roulet-Dubonnet"] __license__ = "GPLv3" __version__ = "0.3" __status__ = "Development" import struct from copy import copy class ParsingException(Exception): def __init__(self, *args): Exception.__init__(self, *args) def _get_data(data, fmt, names): """ fill data into a dictionary data is data from robot packet fmt is struct format, but with added A for arrays and no support for numerical in fmt names args are strings used to store values """ tmpdata = copy(data) fmt = fmt.strip() # space may confuse us d = dict() i = 0 j = 0 while j < len(fmt) and i < len(names): f = fmt[j] if f in (" ", "!", ">", "<"): j += 1 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 arraysize = len(tmpdata) 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]] 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 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]] tmpdata = tmpdata[fmtsize:] j += 1 i += 1 return d def get_header(data): return struct.unpack("!iB", data[0:5]) def analyze_header(data): """ read first 5 bytes and return complete packet """ if not len(data) >= 5: raise ParsingException("Packet size %s smaller than header size (5 bytes)" % len(data)) else: psize, ptype = get_header(data) 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)) return psize, ptype, data[:psize], data[psize:] def find_first_packet(data): """ find the first complete packet in a string returns None if none found """ counter = 0 while True: if len(data) >= 5: psize, ptype = get_header(data) if psize < 5 or psize > 500 or ptype != 16: data = data[1:] elif len(data) > psize: if counter: print("Had to remove % bytes of garbage at begining of packet" % counter) #ok we we have somehting which looks like a packet" return (data[:psize], data[psize:]) else: #packet is not complete return None else: return None def parse(data): """ parse a packet from the UR socket and return a dictionary with the data """ allData = {} #print "Total size ", len(data) while data: psize, ptype, pdata, data = analyze_header(data) #print "We got packet with size %i and type %s" % (psize, ptype) if ptype == 16: allData["SecondaryClientData"] = _get_data(pdata, "!iB", ("size", "type")) data = (pdata + data)[5:] # This is the total size so we resend data to parser elif ptype == 0: allData["RobotModeData"] = _get_data(pdata, "!iBQ???????Bd", ("size", "type", "timestamp", "isRobotConnected", "isRealRobotEnabled", "isPowerOnRobot", "isEmergencyStopped", "isSecurityStopped", "isProgramRunning", "isProgramPaused", "robotMode", "speedFraction")) elif ptype == 1: tmpstr = ["size", "type"] for i in range(0, 6): tmpstr += ["q_actual%s" % i, "q_target%s" % i, "qd_actual%s" % i, "I_actual%s" % i, "V_actual%s" % i, "T_motor%s" % i ,"T_micro%s" % i, "jointMode%s" % i] allData["JointData"] = _get_data(pdata, "!iB dddffffB dddffffB dddffffB dddffffB dddffffB dddffffB", tmpstr) elif ptype == 4: allData["CartesianInfo"] = _get_data(pdata, "iBdddddd", ("size", "type", "X", "Y", "Z", "Rx", "Ry", "Rz")) elif ptype == 5: allData["LaserPointer(OBSOLETE)"] = _get_data(pdata, "iBddd" , ("size", "type")) elif ptype == 3: allData["MasterBoardData"] = _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"] = _get_data(pdata, "iBbbddfBffB" , ("size", "type", "analoginputRange2", "analoginputRange3", "analogInput2", "analogInput3", "toolVoltage48V", "toolOutputVoltage", "toolCurrent", "toolTemperature", "toolMode" )) elif ptype == 20: tmp = _get_data(pdata, "!iB Qbb", ("size", "type", "timestamp", "source", "robotMessageType")) if tmp["robotMessageType"] == 3: allData["VersionMessage"] = _get_data(pdata, "!iBQbb bAbBBiAb", ("size", "type", "timestamp", "source", "robotMessageType", "projectNameSize", "projectName", "majorVersion", "minorVersion", "svnRevision", "buildDate")) elif tmp["robotMessageType"] == 6: allData["robotCommMessage"] = _get_data(pdata, "!iBQbb iiAc", ("size", "type", "timestamp", "source", "robotMessageType", "code", "argument", "messageText")) elif tmp["robotMessageType"] == 1: allData["labelMessage"] = _get_data(pdata, "!iBQbb iAc", ("size", "type", "timestamp", "source", "robotMessageType", "id", "messageText")) elif tmp["robotMessageType"] == 2: allData["popupMessage"] = _get_data(pdata, "!iBQbb ??BAcAc", ("size", "type", "timestamp", "source", "robotMessageType", "warning", "error", "titleSize", "messageTitle", "messageText")) elif tmp["robotMessageType"] == 0: allData["messageText"] = _get_data(pdata, "!iBQbb Ac", ("size", "type", "timestamp", "source", "robotMessageType", "messageText")) elif tmp["robotMessageType"] == 8: allData["varMessage"] = _get_data(pdata, "!iBQbb iiBAcAc", ("size", "type", "timestamp", "source", "robotMessageType", "code", "argument", "titleSize", "messageTitle", "messageText")) elif tmp["robotMessageType"] == 7: allData["keyMessage"] = _get_data(pdata, "!iBQbb iiBAcAc", ("size", "type", "timestamp", "source", "robotMessageType", "code", "argument", "titleSize", "messageTitle", "messageText")) elif tmp["robotMessageType"] == 5: allData["keyMessage"] = _get_data(pdata, "!iBQbb iiAc", ("size", "type", "timestamp", "source", "robotMessageType", "code", "argument", "messageText")) else: print("Message type parser not implemented ", tmp) else: print("Uknown packet type %s with size %s" % (ptype, psize)) return allData