estimator.py 文件源码

def __init__(self):
        rospy.Subscriber('/mavros/local_position/pose', PoseStamped, self.pose_callback, queue_size=1)
        rospy.Subscriber('/marker_data', MarkerArray, self.marker_callback, queue_size=1)

        self.vision_position_pub = rospy.Publisher('/mavros/vision_pose/pose', PoseStamped, queue_size=1)
        self.offset_pub = rospy.Publisher('~offset', PointStamped, queue_size=1)
        self.position_fcu_pub = rospy.Publisher('~position/fcu', PoseStamped, queue_size=1)
        self.pose_pub = rospy.Publisher('~position/marker_map', PoseStamped, queue_size=1)

        self.vision_position_message = PoseStamped()

        self.position_fcu_message = PoseStamped()
        self.position_fcu_message.header.frame_id = 'vision_fcu'

        self.pose_message = PoseStamped()
        self.pose_message.header.frame_id = 'marker_map'

        self.marker_position_offset = PointStamped()