localize.py 文件源码

def range_cb(self, rng):
        self.ranges[rng.header.frame_id] = rng.range
        try:
            trans, _ = self.listener.lookupTransform(
                self.frame_id, rng.header.frame_id, rospy.Time(0))
            self.tag_pos[rng.header.frame_id] = np.array(trans[:3])
        except:
            return

        if len(self.ranges.values()) == 6 and len(self.tag_pos.values()) == 6:
            pos = self.find_xyz()
            pos_3d = self.find_position_3d()
            self.altitude_pub(pos[2])
            self.publish_position(
                pos, self.ps_pub, self.ps_cov_pub, self.cov)
            self.publish_position(
                pos_3d, self.ps_pub_3d, self.ps_cov_pub_3d, self.cov)
            self.ranges = dict()