Exemple #1
0
	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())