adeck/python-urx-getl-rt
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

cleanup and rename some methods-> breaks existing code

Browse Source
This commit is contained in:
Olivier R-D 2015-03-23 15:03:25 +01:00
parent 0c8db9c302
commit ae0b56c644
2 changed files with 101 additions and 120 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

156
urx/robot.py
View File

@ -84,7 +84,7 @@ class URRobot(object):
length of force vector returned by get_tcp_force length of force vector returned by get_tcp_force
if wait==True, waits for next packet before returning if wait==True, waits for next packet before returning
""" """
tcpf = self.get_tcp_force( wait) tcpf = self.get_tcp_force(wait)
force = 0 force = 0
for i in tcpf: for i in tcpf:
force += i**2 force += i**2
@ -163,7 +163,7 @@ class URRobot(object):
""" """
get digital output get digital output
""" """
return self.secmon.get_digital_out(val , wait=wait) return self.secmon.get_digital_out(val, wait=wait)
def set_analog_out(self, output, val): def set_analog_out(self, output, val):
""" """
@ -179,22 +179,15 @@ class URRobot(object):
prog = "set_tool_voltage(%s)" % (val) prog = "set_tool_voltage(%s)" % (val)
self.send_program(prog) self.send_program(prog)
def wait_for_move(self, radius=0, target=None): def wait_for_move(self, radius=0, Target=None):
""" """
wait until a move is completed wait until a move is completed
if radius is not 0, returns when tcp is radius closed to target(using mathd3d dist method) radius and target args are ignored
""" """
self.logger.debug("Waiting for move completion") 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 a physical move 0.5s is very short
for _ in range(3): for _ in range(3):
self.secmon.wait() self.secmon.wait()
if not radius or not MATH3D:
return self._wait_for_move()
else:
return self._wait_for_move_radius(radius, target)
def _wait_for_move(self):
while True: while True:
if not self.is_running(): if not self.is_running():
raise RobotException("Robot stopped") raise RobotException("Robot stopped")
@ -203,34 +196,13 @@ class URRobot(object):
for i in range(0, 6): for i in range(0, 6):
#Rmq: q_target is an interpolated target we have no control over #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: if abs(jts["q_actual%s"%i] - jts["q_target%s"%i]) > self.joinEpsilon:
self.logger.debug("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)) 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 finished = False
break break
if finished and not self.secmon.is_program_running(): if finished and not self.secmon.is_program_running():
self.logger.debug("move has ended") self.logger.debug("move has ended")
return return
def _wait_for_move_radius(self, radius, target):
print("Wait move with radius")
target = m3d.Transform(target)
while True:
if not self.is_running():
raise RobotException("Robot stopped")
pose = self.get_pose(wait=True)
dist = pose.dist(target)
print("dist is ", dist, radius)
if (dist < radius) or not self.secmon.is_program_running():
self.logger.debug("move has ended")
return
def dist(self, poseA, poseB):
"""
Return the metric distance between two poses as unweighted combined linear
and angular distance.
This would be better not to rely on math3d just for that method...
"""
raise NotImplementedError
def getj(self, wait=False): def getj(self, wait=False):
""" """
get joints position get joints position
@ -325,13 +297,10 @@ class URRobot(object):
""" """
pose = self.secmon.get_cartesian_info(wait) pose = self.secmon.get_cartesian_info(wait)
if pose: if pose:
pose = [pose["X"], pose["Y"], pose["Z"], pose["Rx"], pose["Ry"], pose["Rz"]] pose = [pose["X"], pose["Y"], pose["Z"], pose["Rx"], pose["Ry"], pose["Rz"]]
self.logger.debug("Current pose from robot: " + str(pose)) self.logger.debug("Current pose from robot: " + str(pose))
return pose return pose
def get_pose(self, wait=False):
return self.getl(wait)
def movec(self, pose_via, pose_to, acc=0.01, vel=0.01, radius=0, wait=True): def movec(self, pose_via, pose_to, acc=0.01, vel=0.01, radius=0, wait=True):
""" """
Move Circular: Move to position (circular in tool-space) Move Circular: Move to position (circular in tool-space)
@ -347,7 +316,7 @@ class URRobot(object):
self.wait_for_move(radius, pose_to) self.wait_for_move(radius, pose_to)
return self.getl() return self.getl()
def movels(self, pose_list, acc=0.01, vel=0.01 , radius=0.01, wait=True): 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 Concatenate several movel commands and applies a blending radius
pose_list is a list of pose. pose_list is a list of pose.
@ -359,7 +328,7 @@ class URRobot(object):
for idx, pose in enumerate(pose_list): for idx, pose in enumerate(pose_list):
pose.append(acc) pose.append(acc)
pose.append(vel) pose.append(vel)
if idx != (len(pose_list) -1 ): if idx != (len(pose_list) -1):
pose.append(radius) pose.append(radius)
else: else:
pose.append(0) pose.append(0)
@ -369,13 +338,13 @@ class URRobot(object):
if not wait: if not wait:
return None return None
else: else:
self.wait_for_move() self.wait_for_move(radius=0, target=pose_list[-1])
return self.getl() return self.getl()
def stopl(self, acc = 0.5): def stopl(self, acc=0.5):
self.send_program("stopl(%s)" % acc) self.send_program("stopl(%s)" % acc)
def stopj(self, acc = 1.5): def stopj(self, acc=1.5):
self.send_program("stopj(%s)" % acc) self.send_program("stopj(%s)" % acc)
def stop(self): def stop(self):
@ -449,14 +418,12 @@ class Robot(URRobot):
self.add_csys(name, matrix) self.add_csys(name, matrix)
self.csys = self.csys_dict[name] self.csys = self.csys_dict[name]
def orient(self, orient, acc=None, vel=None, radius=0, wait=True): def set_orientation(self, orient, acc=None, vel=None, radius=0, wait=True):
if type(orient) is not m3d.Orientation: if type(orient) is not m3d.Orientation:
orient = m3d.Orientation(orient) orient = m3d.Orientation(orient)
trans = self.get_transform() trans = self.get_pose()
trans.orient = orient trans.orient = orient
self.apply_transform(trans, acc, vel, radius, wait=wait) self.set_pose(trans, acc, vel, radius, wait=wait)
set_orientation = orient
def translate(self, vect, acc=None, vel=None, radius=0, wait=True): def translate(self, vect, acc=None, vel=None, radius=0, wait=True):
""" """
@ -466,7 +433,7 @@ class Robot(URRobot):
if not type(vect) is m3d.Vector: if not type(vect) is m3d.Vector:
vect = m3d.Vector(vect) vect = m3d.Vector(vect)
t.pos += m3d.Vector(vect) t.pos += m3d.Vector(vect)
return self.add_transform_base(t, acc, vel, radius, wait=wait) return self.add_pose_base(t, acc, vel, radius, wait=wait)
def translate_tool(self, vect, acc=None, vel=None, radius=0, wait=True): def translate_tool(self, vect, acc=None, vel=None, radius=0, wait=True):
""" """
@ -476,7 +443,7 @@ class Robot(URRobot):
if not type(vect) is m3d.Vector: if not type(vect) is m3d.Vector:
vect = m3d.Vector(vect) vect = m3d.Vector(vect)
t.pos += vect t.pos += vect
return self.add_transform_tool(t, acc, vel, radius, wait=wait) return self.add_pose_tool(t, acc, vel, radius, wait=wait)
def set_pos(self, vect, acc=None, vel=None, radius=0, wait=True): def set_pos(self, vect, acc=None, vel=None, radius=0, wait=True):
""" """
@ -485,9 +452,9 @@ class Robot(URRobot):
if not type(vect) is m3d.Vector: if not type(vect) is m3d.Vector:
vect = m3d.Vector(vect) vect = m3d.Vector(vect)
trans = m3d.Transform(self.get_orientation(), m3d.Vector(vect)) trans = m3d.Transform(self.get_orientation(), m3d.Vector(vect))
return self.apply_transform(trans, acc, vel, radius, wait=wait) return self.set_pose(trans, acc, vel, radius, wait=wait)
def apply_transform(self, trans, acc=None, vel=None, radius=0, wait=True, process=False): def set_pose(self, trans, acc=None, vel=None, radius=0, wait=True, process=False):
""" """
move tcp to point and orientation defined by a transformation move tcp to point and orientation defined by a transformation
if process is True, movep is used instead of movel if process is True, movep is used instead of movel
@ -504,25 +471,25 @@ class Robot(URRobot):
pose = URRobot.movep(self, t.pose_vector, acc=acc, vel=vel, wait=wait, radius=radius) pose = URRobot.movep(self, t.pose_vector, acc=acc, vel=vel, wait=wait, radius=radius)
else: else:
pose = URRobot.movel(self, t.pose_vector, acc=acc, vel=vel, wait=wait, radius=radius) 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 != None: #movel does not return anything when wait is False
return self.csys.inverse * m3d.Transform(pose) return self.csys.inverse * m3d.Transform(pose)
def add_transform_base(self, trans, acc=None, vel=None, radius=0, wait=True, process=False): def add_pose_base(self, trans, acc=None, vel=None, radius=0, wait=True, process=False):
""" """
Add transform expressed in base coordinate Add transform expressed in base coordinate
""" """
pose = self.get_transform() pose = self.get_pose()
return self.apply_transform(trans * pose, acc, vel, radius, wait=wait, process=process) return self.set_pose(trans * pose, acc, vel, radius, wait=wait, process=process)
def add_transform_tool(self, trans, acc=None, vel=None, radius=0, wait=True, process=False): def add_pose_tool(self, trans, acc=None, vel=None, radius=0, wait=True, process=False):
""" """
Add transform expressed in tool coordinate Add transform expressed in tool coordinate
""" """
pose = self.get_transform() pose = self.get_pose()
return self.apply_transform(pose * trans, acc, vel, radius, wait=wait, process=process) return self.set_pose(pose * trans, acc, vel, radius, wait=wait, process=process)
def get_transform(self, wait=False): def get_pose(self, wait=False):
""" """
get current transform from base to to tcp get current transform from base to to tcp
""" """
@ -530,18 +497,12 @@ class Robot(URRobot):
trans = self.csys.inverse * m3d.Transform(pose) trans = self.csys.inverse * m3d.Transform(pose)
return trans return trans
def get_pose(self, wait=False):
"""
get current transform from base to to tcp
"""
return self.get_transform(wait)
def get_orientation(self, wait=False): def get_orientation(self, wait=False):
trans = self.get_transform(wait) trans = self.get_pose(wait)
return trans.orient return trans.orient
def get_pos(self, wait=False): def get_pos(self, wait=False):
trans = self.get_transform(wait) trans = self.get_pose(wait)
return trans.pos return trans.pos
def speedl(self, velocities, acc, min_time): def speedl(self, velocities, acc, min_time):
@ -557,7 +518,7 @@ class Robot(URRobot):
""" """
move at given velocities in tool csys until minimum min_time seconds move at given velocities in tool csys until minimum min_time seconds
""" """
pose = self.get_transform() pose = self.get_pose()
v = self.csys.orient * pose.orient * m3d.Vector(velocities[:3]) v = self.csys.orient * pose.orient * m3d.Vector(velocities[:3])
w = self.csys.orient * pose.orient * m3d.Vector(velocities[3:]) w = self.csys.orient * pose.orient * m3d.Vector(velocities[3:])
URRobot.speedl(self, np.concatenate((v.array, w.array)), acc, min_time) URRobot.speedl(self, np.concatenate((v.array, w.array)), acc, min_time)
@ -568,11 +529,11 @@ class Robot(URRobot):
""" """
t = m3d.Transform(pose) t = m3d.Transform(pose)
if relative: if relative:
return self.add_transform_base(t, acc, vel, wait=wait, process=False) return self.add_pose_base(t, acc, vel, wait=wait, process=False)
else: else:
return self.apply_transform(t, acc, vel, radius, wait=wait, process=False) return self.set_pose(t, acc, vel, radius, wait=wait, process=False)
def movels(self, pose_list, acc=0.01, vel=0.01 , radius=0.01, wait=True): 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 Concatenate several movep commands and applies a blending radius
pose_list is a list of pose. pose_list is a list of pose.
@ -599,20 +560,20 @@ class Robot(URRobot):
move linear to given pose in tool coordinate move linear to given pose in tool coordinate
""" """
t = m3d.Transform(pose) t = m3d.Transform(pose)
self.add_transform_tool(t, acc, vel, wait=wait, process=False) self.add_pose_tool(t, acc, vel, wait=wait, process=False)
def movep(self, pose, acc=None, vel=None, radius=0, wait=True, relative=False): def movep(self, pose, acc=None, vel=None, radius=0, wait=True, relative=False):
pose = m3d.Transform(pose) pose = m3d.Transform(pose)
if relative: if relative:
return self.add_transform_base(pose, acc, vel, wait=wait, process=True, radius=radius) return self.add_pose_base(pose, acc, vel, wait=wait, process=True, radius=radius)
else: else:
return self.apply_transform(pose, acc, vel, wait=wait, process=True, radius=radius) return self.set_pose(pose, acc, vel, wait=wait, process=True, radius=radius)
def getl(self, wait=False): def getl(self, wait=False):
""" """
return current transformation from tcp to current csys return current transformation from tcp to current csys
""" """
t = self.get_transform(wait) t = self.get_pose(wait)
return t.pose_vector.tolist() return t.pose_vector.tolist()
def set_gravity(self, vector): def set_gravity(self, vector):
@ -620,27 +581,32 @@ class Robot(URRobot):
vector = vector.list vector = vector.list
return URRobot.set_gravity(self, vector) return URRobot.set_gravity(self, vector)
set_transform = apply_transform #since we have set_pos, set_orient, we should be consistent and use set for tranform to .. def wait_for_move(self, radius, target):
set_pose = apply_transform """
set_pose_tool = add_transform_tool wait until a move is completed
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,
for _ in range(3):
self.secmon.wait()
target = m3d.Transform(target)
while True:
if not self.is_running():
raise RobotException("Robot stopped")
pose = self.get_pose(wait=True)
dist = pose.dist(target)
self.logger.debug("distance to target is: %s, target dist is %s", dist, radius)
if (dist < radius) or not self.secmon.is_program_running():
self.logger.debug("move has ended")
return
if not MATH3D:
Robot = URRobot
if __name__ == "__main__":
logging.basicConfig(level=logging.INFO) #enable logging if not MATH3D:
try: Robot = URRobot
if len(sys.argv) > 1:
host = sys-argv[0]
else:
host = '192.168.1.5'
robot = Robot( host )
r = robot
from IPython import embed
finally:
if "robot" in dir():
robot.cleanup()

65
urx/ursecmon.py
View File

@ -35,6 +35,7 @@ class TimeoutException(Exception):
class ParserUtils(object): class ParserUtils(object):
def __init__(self): def __init__(self):
self.logger = logging.getLogger(__name__) self.logger = logging.getLogger(__name__)
self.is_v30 = False
def parse(self, data): def parse(self, data):
""" """
@ -49,22 +50,27 @@ class ParserUtils(object):
allData["SecondaryClientData"] = self._get_data(pdata, "!iB", ("size", "type")) 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: elif ptype == 0:
allData["RobotModeData"] = self._get_data(pdata, "!iBQ???????Bd", ("size", "type", "timestamp", "isRobotConnected", "isRealRobotEnabled", "isPowerOnRobot", "isEmergencyStopped", "isSecurityStopped", "isProgramRunning", "isProgramPaused", "robotMode", "speedFraction")) # this parses RobotModeData for versions >=3.0 (i.e. 3.0)
if psize == 38:
self.is_v30 = True
allData['RobotModeData'] = self._get_data(pdata, "!IBQ???????BBdd", ("size", "type", "timestamp", "isRobotConnected", "isRealRobotEnabled", "isPowerOnRobot", "isEmergencyStopped", "isSecurityStopped", "isProgramRunning", "isProgramPaused", "robotMode", "controlMode", "speedFraction", "speedScaling"))
else:
allData["RobotModeData"] = self._get_data(pdata, "!iBQ???????Bd", ("size", "type", "timestamp", "isRobotConnected", "isRealRobotEnabled", "isPowerOnRobot", "isEmergencyStopped", "isSecurityStopped", "isProgramRunning", "isProgramPaused", "robotMode", "speedFraction"))
elif ptype == 1: elif ptype == 1:
tmpstr = ["size", "type"] tmpstr = ["size", "type"]
for i in range(0, 6): 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] 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"] = self._get_data(pdata, "!iB dddffffB dddffffB dddffffB dddffffB dddffffB dddffffB", tmpstr) allData["JointData"] = self._get_data(pdata, "!iB dddffffB dddffffB dddffffB dddffffB dddffffB dddffffB", tmpstr)
elif ptype == 4: elif ptype == 4:
allData["CartesianInfo"] = self._get_data(pdata, "iBdddddd", ("size", "type", "X", "Y", "Z", "Rx", "Ry", "Rz")) allData["CartesianInfo"] = self._get_data(pdata, "iBdddddd", ("size", "type", "X", "Y", "Z", "Rx", "Ry", "Rz"))
elif ptype == 5: elif ptype == 5:
allData["LaserPointer(OBSOLETE)"] = self._get_data(pdata, "iBddd" , ("size", "type")) allData["LaserPointer(OBSOLETE)"] = self._get_data(pdata, "iBddd", ("size", "type"))
elif ptype == 3: 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: elif ptype == 2:
allData["ToolData"] = self._get_data(pdata, "iBbbddfBffB" , ("size", "type", "analoginputRange2", "analoginputRange3", "analogInput2", "analogInput3", "toolVoltage48V", "toolOutputVoltage", "toolCurrent", "toolTemperature", "toolMode" )) allData["ToolData"] = self._get_data(pdata, "iBbbddfBffB", ("size", "type", "analoginputRange2", "analoginputRange3", "analogInput2", "analogInput3", "toolVoltage48V", "toolOutputVoltage", "toolCurrent", "toolTemperature", "toolMode"))
#elif ptype == 8: #elif ptype == 8:
#allData["varMessage"] = self._get_data(pdata, "!iBQbb iiBAcAc", ("size", "type", "timestamp", "source", "robotMessageType", "code", "argument", "titleSize", "messageTitle", "messageText")) #allData["varMessage"] = self._get_data(pdata, "!iBQbb iiBAcAc", ("size", "type", "timestamp", "source", "robotMessageType", "code", "argument", "titleSize", "messageTitle", "messageText"))
@ -90,7 +96,7 @@ class ParserUtils(object):
elif tmp["robotMessageType"] == 5: elif tmp["robotMessageType"] == 5:
allData["keyMessage"] = self._get_data(pdata, "!iBQbb iiAc", ("size", "type", "timestamp", "source", "robotMessageType", "code", "argument", "messageText")) allData["keyMessage"] = self._get_data(pdata, "!iBQbb iiAc", ("size", "type", "timestamp", "source", "robotMessageType", "code", "argument", "messageText"))
else: else:
self.logger.debug("Message type parser not implemented ", tmp) self.logger.debug("Message type parser not implemented %s", tmp)
else: else:
self.logger.debug("Unknown packet type %s with size %s" % (ptype, psize)) self.logger.debug("Unknown packet type %s with size %s" % (ptype, psize))
@ -117,7 +123,7 @@ class ParserUtils(object):
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) arraysize = len(tmpdata)
else: # size should be given in last element else: # size should be given in last element
asn = names[i-1] asn = names[i-1]
if not asn.endswith("Size"): if not asn.endswith("Size"):
raise ParsingException("Error, array without size ! %s %s" % (asn, i)) raise ParsingException("Error, array without size ! %s %s" % (asn, i))
else: else:
@ -153,7 +159,7 @@ class ParserUtils(object):
if psize < 5: if psize < 5:
raise ParsingException("Error, declared length of data smaller than its own header(5): ", psize) raise ParsingException("Error, declared length of data smaller than its own header(5): ", psize)
elif psize > len(data): 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:] return psize, ptype, data[:psize], data[psize:]
def find_first_packet(self, data): def find_first_packet(self, data):
@ -170,18 +176,18 @@ class ParserUtils(object):
data = data[1:] data = data[1:]
counter += 1 counter += 1
if counter > limit: if counter > limit:
self.logger.warn("tried {} times to find a packet in data, advertised packet size: {}, type: {}".format(counter, psize, ptype)) self.logger.warn("tried %s times to find a packet in data, advertised packet size: %s, type: %s", counter, psize, ptype)
self.logger.warn("Data length: {}".format(len(data))) self.logger.warn("Data length: {}".format(len(data)))
limit = limit * 10 limit = limit * 10
elif len(data) >= psize: elif len(data) >= psize:
self.logger.debug("Got packet with size {0} and type {1}".format(psize, ptype)) self.logger.debug("Got packet with size %s and type %s", psize, ptype)
if counter: if counter:
self.logger.info("Remove {0} bytes of garbage at begining of packet".format(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:]) return (data[:psize], data[psize:])
else: else:
#packet is not complete #packet is not complete
self.logger.debug("Packet is not complete, advertised size is {0}, received size is {1}, type is {2}".format(psize, len(data), ptype)) self.logger.debug("Packet is not complete, advertised size is %s, received size is %s, type is %s", psize, len(data), ptype)
return None return None
else: else:
#self.logger.debug("data smaller than 5 bytes") #self.logger.debug("data smaller than 5 bytes")
@ -246,29 +252,38 @@ class SecondaryMonitor(Thread):
with self._dictLock: with self._dictLock:
self._dict = tmpdict self._dict = tmpdict
except ParsingException as ex: except ParsingException as ex:
self.logger.warn("Error parsing one packet from urrobot: " + str(ex) ) self.logger.warn("Error parsing one packet from urrobot: " + str(ex))
continue continue
if "RobotModeData" not in self._dict: if "RobotModeData" not in self._dict:
self.logger.warn( "Got a packet from robot without RobotModeData, strange ...") self.logger.warn("Got a packet from robot without RobotModeData, strange ...")
continue continue
self.lastpacket_timestamp = time.time() self.lastpacket_timestamp = time.time()
if self._dict["RobotModeData"]["robotMode"] == 0 \ if self._parser.is_v30:
if self._dict["RobotModeData"]["robotMode"] == 7 \
and self._dict["RobotModeData"]["isRealRobotEnabled"] == True \ and self._dict["RobotModeData"]["isRealRobotEnabled"] == True \
and self._dict["RobotModeData"]["isEmergencyStopped"] == False \ and self._dict["RobotModeData"]["isEmergencyStopped"] == False \
and self._dict["RobotModeData"]["isSecurityStopped"] == False \ and self._dict["RobotModeData"]["isSecurityStopped"] == False \
and self._dict["RobotModeData"]["isRobotConnected"] == True \ and self._dict["RobotModeData"]["isRobotConnected"] == True \
and self._dict["RobotModeData"]["isPowerOnRobot"] == True: and self._dict["RobotModeData"]["isPowerOnRobot"] == True:
self.running = True self.running = True
else: else:
if self.running == True: if self._dict["RobotModeData"]["robotMode"] == 0 \
self.logger.error("Robot not running: " + str( self._dict["RobotModeData"])) and self._dict["RobotModeData"]["isRealRobotEnabled"] == True \
self.running = False and self._dict["RobotModeData"]["isEmergencyStopped"] == False \
with self._dataEvent: and self._dict["RobotModeData"]["isSecurityStopped"] == False \
#print("X: new data") and self._dict["RobotModeData"]["isRobotConnected"] == True \
self._dataEvent.notifyAll() and self._dict["RobotModeData"]["isPowerOnRobot"] == True:
self.running = True
else:
if self.running == True:
self.logger.error("Robot not running: " + str(self._dict["RobotModeData"]))
self.running = False
with self._dataEvent:
#print("X: new data")
self._dataEvent.notifyAll()
def _get_data(self): def _get_data(self):
""" """
@ -294,7 +309,7 @@ class SecondaryMonitor(Thread):
with self._dataEvent: with self._dataEvent:
self._dataEvent.wait(timeout) self._dataEvent.wait(timeout)
if tstamp == self.lastpacket_timestamp: if tstamp == self.lastpacket_timestamp:
raise TimeoutException("Did not receive a valid data packet from robot in {}".format(timeout) ) raise TimeoutException("Did not receive a valid data packet from robot in {}".format(timeout))
def get_cartesian_info(self, wait=False): def get_cartesian_info(self, wait=False):
if wait: if wait:
@ -329,7 +344,7 @@ class SecondaryMonitor(Thread):
with self._dictLock: with self._dictLock:
output = self._dict["MasterBoardData"]["digitalOutputBits"] output = self._dict["MasterBoardData"]["digitalOutputBits"]
mask = 1 << nb mask = 1 << nb
if (output & mask): if output & mask:
return 1 return 1
else: else:
return 0 return 0
@ -340,7 +355,7 @@ class SecondaryMonitor(Thread):
with self._dictLock: with self._dictLock:
output = self._dict["MasterBoardData"]["digitalInputBits"] output = self._dict["MasterBoardData"]["digitalInputBits"]
mask = 1 << nb mask = 1 << nb
if (output & mask): if output & mask:
return 1 return 1
else: else:
return 0 return 0