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()