localize.py 文件源码

def __init__(self):
        self.frame_id = rospy.get_param("~frame_id", "map")
        cov_x = rospy.get_param("~cov_x", 0.6)
        cov_y = rospy.get_param("~cov_y", 0.6)
        cov_z = rospy.get_param("~cov_z", 0.6)
        self.cov = self.cov_matrix(cov_x, cov_y, cov_z)
        self.ps_pub = rospy.Publisher(
            POSE_TOPIC, PoseStamped, queue_size=1)
        self.ps_cov_pub = rospy.Publisher(
            POSE_COV_TOPIC, PoseWithCovarianceStamped, queue_size=1)
        self.ps_pub_3d = rospy.Publisher(
            POSE_TOPIC_3D, PoseStamped, queue_size=1)
        self.ps_cov_pub_3d = rospy.Publisher(
            POSE_COV_TOPIC_3D, PoseWithCovarianceStamped, queue_size=1)
        self.last = None
        self.listener = tf.TransformListener()
        self.tag_range_topics = rospy.get_param("~tag_range_topics")
        self.subs = list()
        self.ranges = dict()
        self.tag_pos = dict()
        self.altitude = 0.0
        self.last_3d = None
        for topic in self.tag_range_topics:
            self.subs.append(rospy.Subscriber(topic, Range, self.range_cb))