$search
00001 #!/usr/bin/env python 00002 import roslib; roslib.load_manifest('sr_utilities') 00003 import rospy 00004 from std_msgs.msg import Float64 00005 import thread 00006 00007 00008 class FilterMessages: 00009 def __init__(self): 00010 rospy.init_node('mean_filter', anonymous=True) 00011 00012 if rospy.has_param('~nb_occ'): 00013 nb_occurence = rospy.get_param('~nb_occ') 00014 else: 00015 nb_occurence = 5 00016 rospy.core.logwarn("N number of occurence provided, using 5 as default") 00017 00018 if rospy.has_param('~in_topic'): 00019 input_topic_param = rospy.get_param('~in_topic') 00020 self.subs = rospy.Subscriber(input_topic_param, Float64, self.callback) 00021 self.pub = rospy.Publisher(input_topic_param+"_filtered", Float64) 00022 else: 00023 rospy.core.logerr("No input topic provided, not starting filtering") 00024 return 00025 00026 self.queue=[] 00027 self.queue_len=nb_occurence 00028 self.mean=Float64(0.0); 00029 00030 rospy.spin() 00031 00032 def compute_mean(self): 00033 mean=0.0 00034 for val in range(self.queue_len): 00035 mean=mean+self.queue[val].data 00036 00037 return mean/self.queue_len 00038 00039 00040 def callback(self, data): 00041 00042 self.queue.append(data) 00043 00044 if len(self.queue)>=self.queue_len: 00045 self.mean=Float64(self.compute_mean()) 00046 self.queue.pop(0) 00047 00048 self.pub.publish(self.mean) 00049 00050 00051 if __name__ == '__main__': 00052 filter = FilterMessages()