class MavModel(object): def __init__(self, maxLinearSpeed, maxAngularSpeed, correctionMatrix): self.dest = PoseModel() #Destination point in World. self.destENU = PoseModel() #Destination in ENU frame. self.camera = PoseModel() #Camera position in World. self.body = PoseModel() #Body position in World. Y is aligned with Pixhawk front. Corrected position of camera. self.enu = PoseModel() #ENU frame. Origin of this frame is body position. Y is aligned with geographical North self.localEnuPose = PoseModel() self.headingAngle = None #angle from pixhawk front to North in CCW(right-hand). The number is represented in radians self.maxLinearSpeed = maxLinearSpeed self.maxAngularSpeed = maxAngularSpeed self.correctionMatrix = correctionMatrix self.twist = MavTwist(self) #eof def runPoseCorrection(self): self.body.setMatrix(self.camera.getMatrix()) if(self.correctionMatrix is not None): self.body.transformByMatrix(self.correctionMatrix) return self #eof def runCalculation(self): if(self.calENU() and self.calDestENU()): self.twist.calAngular().calLinear() else: self.twist.reset() return self #eof def calDestENU(self): if(self.enu.isNone() or self.dest.isNone() or self.localEnuPose.isNone()): return False else: enuInverse = self.localEnuPose.getInverseMatrix() self.destENU.setMatrix(numpy.dot(enuInverse, self.dest.getMatrix())) return True #eof def calENU(self): if(self.body.isNone() or self.headingAngle is None): return False else: br = self.body.getRotationAroundZ() q = rotation_matrix(br+self.headingAngle, (0,0,1)) t = translation_from_matrix(self.body.getMatrix()) t = translation_matrix(t) self.enu.setMatrix(t).transformByMatrix(q) return True #eoc
class Controller:
def __init__(self):
rospy.Subscriber("/mavros/local_position/pose", PoseStamped, self.localCb)
rospy.Subscriber("/itech_ros/mavros_offboard/enu", PoseStamped, self.enuCb)
self.localEnuPub = rospy.Publisher(util.topicName("mavros_local_pose_fusion", "local_enu"), PoseStamped, queue_size=10)
self.offsetPub = rospy.Publisher(util.topicName("mavros_local_pose_fusion", "offset"), PointStamped, queue_size=10)
self.offset = None
self.localPose = PoseModel()
self.localEnuPose = PoseModel()
#tick posee are the poses at the time that we get positioning data.
self.enuTickPose = PoseModel()
self.localTickPose = PoseModel()
self.r = euler_matrix(0, 0, math.pi/-2)
#eof
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())
#eof
def calculateOffset(self):
if(not (self.enuTickPose.isNone() or self.localTickPose.isNone())):
t = self.localTickPose.getTranslation()
q = self.enuTickPose.getQuaternion()
t = translation_matrix(t)
q = quaternion_matrix(q)
rp = PoseModel(numpy.dot(q,t))
t1 = rp.getTranslation()
t2 = self.enuTickPose.getTranslation()
self.offset = [t1[0]-t2[0], t1[1]-t2[1], t1[2]-t2[2]]
rospy.logwarn(self.offset)
#eof
def enuCb(self, data):
self.enuTickPose.setPoseStamped(data)
self.calculateOffset()
self.localTickPose.setMatrix(self.localPose.getMatrix())
#eof
#eoc
