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

26
urx/ursecmon.py
View File

@ -193,7 +193,7 @@ class SecondaryMonitor(Thread):
self._dataEvent = Condition()
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):
"""
@ -226,7 +226,7 @@ class SecondaryMonitor(Thread):
with self._dictLock:
self._dict = self._parser.parse(data)
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
if "RobotModeData" not in self._dict:
@ -244,6 +244,7 @@ class SecondaryMonitor(Thread):
if self.running == True:
self.logger.error("Robot not running: " + str( self._dict["RobotModeData"]))
self.running = False
with self._dataEvent:
self._dataEvent.notifyAll()
def _get_data(self):
@ -259,33 +260,46 @@ class SecondaryMonitor(Thread):
tmp = self._s_secondary.recv(1024)
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:
if "CartesianInfo" in self._dict:
return self._dict["CartesianInfo"]
else:
return None
def getAllData(self):
def getAllData(self, wait=False):
"""
return last data obtained from robot in dictionnary format
"""
if wait:
self.wait()
with self._dictLock:
return self._dict.copy()
def getJointData(self):
def getJointData(self, wait=False):
if wait:
self.wait()
with self._dictLock:
if "JointData" in self._dict:
return self._dict["JointData"]
else:
return None
def isProgramRunning(self):
def isProgramRunning(self, wait=False):
"""
return True if robot is executing a program
Rmq: The refresh rate is only 10Hz so the information may be outdated
"""
if wait:
self.wait()
with self._dictLock:
return self._dict["RobotModeData"]["isProgramRunning"]

36
urx/urx.py
View File

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