topic_filter.py 文件源码

def filter_topic(src_topic, dest_topic, topic_type):
    """
    Publishes a measured data point given the raw value by applying the EWMA function to the data.
    :param src_topic: String? - Source topic signal to be filtered
    :param dest_topic: String? - Output topic to publish new data to
    :param topic_type: The data type of the topic, aka Float64, Int32, etc
    :return: sub, pub : subscribed topic (raw measured value) and published topic (filtered (smoothed) value)
    """
    rospy.loginfo("Filtering topic {} to topic {}".format(
        src_topic, dest_topic
    ))
    pub = rospy.Publisher(dest_topic, topic_type, queue_size=10)
    f = EWMA(0.3)
    def callback(src_item):
        value = src_item.data
        # If the value is our magic number, leave it alone
        if value in SENTINELS:
            dest_item = value
        else:
            f(src_item.data)
            dest_item = topic_type(f.average)
        pub.publish(dest_item)
    sub = rospy.Subscriber(src_topic, topic_type, callback)
    return sub, pub