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

fixed error in events, fixed bug in logging

Browse Source
This commit is contained in:
Olivier R-D 2013-02-08 14:07:23 +01:00
parent 4ab7fd882c
commit cbbce72b18
2 changed files with 41 additions and 23 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
urx/ursecmon.py
View File

@ -193,7 +193,7 @@ class SecondaryMonitor(Thread):
self._dataEvent = Condition() self._dataEvent = Condition()
self.start() self.start()
self._dataEvent.wait() # make sure we got some data before someone calls us self.wait()# make sure we got some data before someone calls us
def sendProgram(self, prog): def sendProgram(self, prog):
""" """
@ -226,7 +226,7 @@ class SecondaryMonitor(Thread):
with self._dictLock: with self._dictLock:
self._dict = self._parser.parse(data) self._dict = self._parser.parse(data)
except ParsingException as ex: except ParsingException as ex:
self.logger.warn("Error parsing data 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:
@ -244,7 +244,8 @@ class SecondaryMonitor(Thread):
if self.running == True: if self.running == True:
self.logger.error("Robot not running: " + str( self._dict["RobotModeData"])) self.logger.error("Robot not running: " + str( self._dict["RobotModeData"]))
self.running = False self.running = False
self._dataEvent.notifyAll() with self._dataEvent:
self._dataEvent.notifyAll()
def _get_data(self): def _get_data(self):
""" """
@ -259,33 +260,46 @@ class SecondaryMonitor(Thread):
tmp = self._s_secondary.recv(1024) tmp = self._s_secondary.recv(1024)
self._dataqueue += tmp self._dataqueue += tmp
def getCartesianInfo(self): def wait(self):
"""
wait for next data packet from robot
"""
def getCartesianInfo(self, wait=False):
if wait:
self.wait()
with self._dictLock: with self._dictLock:
if "CartesianInfo" in self._dict: if "CartesianInfo" in self._dict:
return self._dict["CartesianInfo"] return self._dict["CartesianInfo"]
else: else:
return None return None
def getAllData(self): def getAllData(self, wait=False):
""" """
return last data obtained from robot in dictionnary format return last data obtained from robot in dictionnary format
""" """
if wait:
self.wait()
with self._dictLock: with self._dictLock:
return self._dict.copy() return self._dict.copy()
def getJointData(self): def getJointData(self, wait=False):
if wait:
self.wait()
with self._dictLock: with self._dictLock:
if "JointData" in self._dict: if "JointData" in self._dict:
return self._dict["JointData"] return self._dict["JointData"]
else: else:
return None return None
def isProgramRunning(self): def isProgramRunning(self, wait=False):
""" """
return True if robot is executing a program return True if robot is executing a program
Rmq: The refresh rate is only 10Hz so the information may be outdated Rmq: The refresh rate is only 10Hz so the information may be outdated
""" """
if wait:
self.wait()
with self._dictLock: with self._dictLock:
return self._dict["RobotModeData"]["isProgramRunning"] return self._dict["RobotModeData"]["isProgramRunning"]

36
urx/urx.py
View File

@ -96,6 +96,8 @@ class URRobot(object):
""" """
def __init__(self, host, useRTInterface=False): def __init__(self, host, useRTInterface=False):
self.logger = logging.getLogger(self.__class__.__name__) self.logger = logging.getLogger(self.__class__.__name__)
if len(logging.root.handlers) == 0: #dirty hack
logging.basicConfig()
self.host = host self.host = host
self.logger.info("Opening secondary monitor socket") self.logger.info("Opening secondary monitor socket")
@ -269,11 +271,11 @@ class URRobot(object):
if self._eq(currentjoints, joints) and not self.secmon.isProgramRunning(): if self._eq(currentjoints, joints) and not self.secmon.isProgramRunning():
return currentjoints return currentjoints
def getj(self): def getj(self, wait=False):
""" """
get joints position get joints position
""" """
jts = self.secmon.getJointData() jts = self.secmon.getJointData(wait)
return [jts["q_actual0"], jts["q_actual1"], jts["q_actual2"], jts["q_actual3"], jts["q_actual4"], jts["q_actual5"]] return [jts["q_actual0"], jts["q_actual1"], jts["q_actual2"], jts["q_actual3"], jts["q_actual4"], jts["q_actual5"]]
@ -297,19 +299,19 @@ class URRobot(object):
if not wait: if not wait:
return None return None
else: else:
time.sleep(0.05)# it is important to sleep since robot may takes a while to get into running state
while True: while True:
time.sleep(0.05)# it is important to sleep since robot may takes a while to get into running state
if not self.isRunning():#FIXME add more tests if not self.isRunning():#FIXME add more tests
raise RobotException("Robot stopped") raise RobotException("Robot stopped")
pose = self.getl() pose = self.getl(wait=True)
if self._eqpose(pose, tpose) and not self.secmon.isProgramRunning(): if self._eqpose(pose, tpose) and not self.secmon.isProgramRunning():
return pose return pose
def getl(self): def getl(self, wait=False):
""" """
get TCP position get TCP position
""" """
pose = self.secmon.getCartesianInfo() pose = self.secmon.getCartesianInfo(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))
@ -374,6 +376,8 @@ class Robot(object):
def __init__(self, host, useRTInterface=False): def __init__(self, host, useRTInterface=False):
self.robot = URRobot(host, useRTInterface) self.robot = URRobot(host, useRTInterface)
self.logger = logging.getLogger(self.__class__.__name__) self.logger = logging.getLogger(self.__class__.__name__)
if len(logging.root.handlers) == 0: #dirty hack
logging.basicConfig()
logging.basicConfig(level=logging.DEBUG, format='%(asctime)s - %(name)s - %(levelname)s - %(message)s') logging.basicConfig(level=logging.DEBUG, format='%(asctime)s - %(name)s - %(levelname)s - %(message)s')
self.default_linear_acceleration = 0.01 self.default_linear_acceleration = 0.01
self.default_linear_velocity = 0.01 self.default_linear_velocity = 0.01
@ -427,17 +431,17 @@ class Robot(object):
pose = self.get_transform() pose = self.get_transform()
return self.apply_transform(trans * pose, acc, vel, wait) return self.apply_transform(trans * pose, acc, vel, wait)
def get_transform(self): def get_transform(self, wait=False):
pose = self.robot.getl() pose = self.robot.getl(wait)
trans = self.inverse * math3d.Transform(pose) trans = self.inverse * math3d.Transform(pose)
return trans return trans
def get_orientation(self): def get_orientation(self, wait=False):
trans = self.get_transform() trans = self.get_transform(wait)
return trans.orient return trans.orient
def get_pos(self): def get_pos(self, wait=False):
trans = self.get_transform() trans = self.get_transform(wait)
return trans.pos return trans.pos
def movel(self, pose, acc=None, vel=None, wait=True): def movel(self, pose, acc=None, vel=None, wait=True):
@ -454,8 +458,8 @@ class Robot(object):
""" """
pass pass
def getl(self): def getl(self, wait=False):
t = self.get_transform() t = self.get_transform(wait)
return t.pose_vector return t.pose_vector
def is_running(self): def is_running(self):
@ -481,8 +485,8 @@ class Robot(object):
""" """
self.robot.movej(joints, acc, vel, wait) self.robot.movej(joints, acc, vel, wait)
def getj(self): def getj(self, wait=False):
return self.robot.getj() return self.robot.getj(wait)
def movejrel(self, joints, acc=0.1, vel=0.05, wait=True): def movejrel(self, joints, acc=0.1, vel=0.05, wait=True):
""" """