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)
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)
class AnchorMarker: def __init__(self, mid, p=[0,0,0], o=[0,0,0,1]): self.pose = PoseModel() self.pose.id = mid self.pose.setMatrix(util.transformationMatrix(p, o)) #eof def getCameraMatrixByMarker(self, marker): r = PoseModel().setMarker(marker).getInverseMatrix() return self.pose.getTransformedMatrix(r) #eof #eoc
def __init__(self, anchors):
self.anchors = anchors
self.poseStampedPublisher = rospy.Publisher(util.topicName("marker_pose", "pose"), PoseStamped, queue_size=10)
self.markerArrayPublisher = rospy.Publisher(util.topicName("marker_pose", "markers"), PoseArray, queue_size=10)
self.poseSepPublisher = rospy.Publisher(util.topicName("marker_pose", "pose_sep"), PoseArray, queue_size=10)
rospy.Subscriber("cam2/camera/apriltags_marker", MarkerArray, self.markerCb)
self.cameraPose = PoseModel()
self.poses = []
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)
def __init__(self, mid, p=[0,0,0], o=[0,0,0,1]): self.pose = PoseModel() self.pose.id = mid self.pose.setMatrix(util.transformationMatrix(p, o))
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
class PoseController:
"""
Getting camera position in space based on known position of markers.
:subscribe:
/camera/apriltags_marker
:publish:
marker_pose/pose
marker_pose/markers
"""
def __init__(self, anchors):
self.anchors = anchors
self.poseStampedPublisher = rospy.Publisher(util.topicName("marker_pose", "pose"), PoseStamped, queue_size=10)
self.markerArrayPublisher = rospy.Publisher(util.topicName("marker_pose", "markers"), PoseArray, queue_size=10)
self.poseSepPublisher = rospy.Publisher(util.topicName("marker_pose", "pose_sep"), PoseArray, queue_size=10)
rospy.Subscriber("cam2/camera/apriltags_marker", MarkerArray, self.markerCb)
self.cameraPose = PoseModel()
self.poses = []
#eof
def markerCb(self, data):
if self.calPosition(data.markers):
self.publish()
#eof
def calPosition(self, markerArray):
matrices = []
poses = []
for marker in markerArray:
if self.anchors.has_key(marker.id):
if(self.isValidMarker(marker)):
anchor = self.anchors.get(marker.id)
mat = anchor.getCameraMatrixByMarker(marker)
matrices.append(mat)
poses.append(PoseModel(mat).getPose())
else:
rospy.logwarn("Invalid Marker. id: "+str(marker.id))
else:
rospy.logwarn("Unknown Marker. id: "+str(marker.id))
#eo for
if len(matrices)>0:
self.cameraPose.setMatrix(sum(matrices)/len(matrices))
self.poses = poses
return True
else:
return False
#eof
def publish(self):
self.publishPose()
self.publishMarkers()
#eof
def publishPose(self):
if self.cameraPose is not None:
self.poseStampedPublisher.publish(self.cameraPose.getPoseStamped())
header = Header(0, rospy.rostime.get_rostime(), "world")
self.poseSepPublisher.publish(PoseArray(header, self.poses))
#eof
def publishMarkers(self):
poseArr = []
header = Header(0, rospy.rostime.get_rostime(), "world")
for a in self.anchors:
poseArr.append(self.anchors[a].pose.getPose())
self.markerArrayPublisher.publish(PoseArray(header, poseArr))
#eof
def isValidMarker(self, marker):
m = util.markerTransformationMatrix(marker)
i = numpy.identity(4)
return not numpy.allclose(m, i)
#eof
#eoc