def localCb(self, data): self.localPose.setPoseStamped(data) if(not (self.enuTickPose.isNone() or self.offset is None)): t = self.localPose.getTranslation() q = self.enuTickPose.getQuaternion() q = quaternion_matrix(q) t = translation_matrix(t) self.localEnuPose.setMatrix(numpy.dot(q,t)) t = self.localEnuPose.getTranslation() t[0] -= self.offset[0] t[1] -= self.offset[1] t[2] -= self.offset[2] q = self.localEnuPose.getQuaternion() self.localEnuPose.setTranslationQuaternion(t, q) p = PointStamped() p.point.x = self.offset[0] p.point.y = self.offset[1] p.point.z = self.offset[2] p.header = Header(0, rospy.rostime.get_rostime(), "world") self.offsetPub.publish(p) self.localEnuPub.publish(self.localEnuPose.getPoseStamped())