uwb_tracker_node.py 文件源码

def __init__(self):
        """Initialize tracker.
        """
        self._read_configuration()

        self.estimates = {}
        self.estimate_times = {}
        self.ikf_prev_outlier_flags = {}
        self.ikf_outlier_counts = {}
        self.outlier_thresholds = {}

        rospy.loginfo("Receiving raw multi-range messages from: {}".format(self.uwb_multi_range_topic))
        rospy.loginfo("Publishing tracker messages to {}".format(self.uwb_tracker_topic))
        rospy.loginfo("Publishing tracker transform as {} -> {}".format(self.tracker_frame, self.target_frame))

        # ROS publishers and subscribers
        self.tracker_frame = self.tracker_frame
        self.target_frame = self.target_frame
        self.uwb_pub = rospy.Publisher(self.uwb_tracker_topic, uwb.msg.UWBTracker, queue_size=1)
        self.tf_broadcaster = tf.TransformBroadcaster()
        self.uwb_multi_range_sub = rospy.Subscriber(self.uwb_multi_range_topic, uwb.msg.UWBMultiRangeWithOffsets,
                                                    self.handle_multi_range_message)