def __init__(self, ic, prefix):
'''
Pose3d Contructor.
Exits When it receives a Exception diferent to Ice.ConnectionRefusedException
@param ic: Ice Communicator
@param prefix: prefix name of client in config file
@type ic: Ice Communicator
@type prefix: String
'''
self.lock = threading.Lock()
self.pose = Pose3d()
try:
base = ic.propertyToProxy(prefix + ".Proxy")
self.proxy = jderobot.Pose3DPrx.checkedCast(base)
prop = ic.getProperties()
self.update()
if not self.proxy:
print('Interface ' + prefix + ' not configured')
except Ice.ConnectionRefusedException:
print(prefix + ': connection refused')
except:
traceback.print_exc()
exit(-1)
def odometry2Pose3D(odom):
'''
Translates from ROS Odometry to JderobotTypes Pose3d.
@param odom: ROS Odometry to translate
@type odom: Odometry
@return a Pose3d translated from odom
'''
pose = Pose3d()
ori = odom.pose.pose.orientation
pose.x = odom.pose.pose.position.x
pose.y = odom.pose.pose.position.y
pose.z = odom.pose.pose.position.z
#pose.h = odom.pose.pose.position.h
pose.yaw = quat2Yaw(ori.w, ori.x, ori.y, ori.z)
pose.pitch = quat2Pitch(ori.w, ori.x, ori.y, ori.z)
pose.roll = quat2Roll(ori.w, ori.x, ori.y, ori.z)
pose.q = [ori.w, ori.x, ori.y, ori.z]
pose.timeStamp = odom.header.stamp.secs + (odom.header.stamp.nsecs * 1e-9)
return pose
def __init__(self, topic):
'''
ListenerPose3d Constructor.
@param topic: ROS topic to subscribe
@type topic: String
'''
self.topic = topic
self.data = Pose3d()
self.sub = None
self.lock = threading.Lock()
self.start()
def update(self):
'''
Updates Pose3d.
'''
pos = Pose3d()
if self.hasproxy():
pose = self.proxy.getPose3DData()
pos.yaw = self.quat2Yaw(pose.q0, pose.q1, pose.q2, pose.q3)
pos.pitch = self.quat2Pitch(pose.q0, pose.q1, pose.q2, pose.q3)
pos.roll = self.quat2Roll(pose.q0, pose.q1, pose.q2, pose.q3)
pos.x = pose.x
pos.y = pose.y
pos.z = pose.z
pos.h = pose.h
pos.q = [pose.q0, pose.q1, pose.q2, pose.q3]
self.lock.acquire()
self.pose = pos
self.lock.release()