ekf.py 文件源码

def range_cb(self, rng):
        self.ranges[rng.header.frame_id] = rng.range
        if len(self.tag_order) < len(self.tag_range_topics):
            self.tag_order.append(rng.header.frame_id)

            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])
                self.uwb_state[len(self.tag_order)-1, :] = \
                    np.array([trans[0], trans[1], trans[2], 0, 0, 0])
            except tf.Exception:
                return