moving_average_filter.py

Go to the documentation of this file.

#!/usr/bin/env python
import roslib; roslib.load_manifest('sr_utilities')
import rospy
from std_msgs.msg import Float64
import thread


class FilterMessages:
    def __init__(self):
        rospy.init_node('mean_filter', anonymous=True)

        if rospy.has_param('~nb_occ'):
                nb_occurence = rospy.get_param('~nb_occ')
        else:
                nb_occurence = 5
                rospy.core.logwarn("N number of occurence provided, using 5 as default")

        if rospy.has_param('~in_topic'):
                input_topic_param = rospy.get_param('~in_topic')
                self.subs = rospy.Subscriber(input_topic_param, Float64, self.callback)
                self.pub = rospy.Publisher(input_topic_param+"_filtered", Float64)        
        else:
                rospy.core.logerr("No input topic provided, not starting filtering")
                return

        self.queue=[]
        self.queue_len=nb_occurence
        self.mean=Float64(0.0);
        
        rospy.spin()

    def compute_mean(self):
        mean=0.0
        for val in range(self.queue_len):
                mean=mean+self.queue[val].data

        return mean/self.queue_len


    def callback(self, data):

        self.queue.append(data)

        if len(self.queue)>=self.queue_len:       
                self.mean=Float64(self.compute_mean())
                self.queue.pop(0)
        
        self.pub.publish(self.mean)
             

if __name__ == '__main__':
    filter = FilterMessages()