Main Page
Namespaces
Classes
Files
File List
File Members
scripts
moving_average_filter.py
Go to the documentation of this file.
1
#!/usr/bin/env python
2
import
rospy
3
from
std_msgs.msg
import
Float64
4
5
6
class
FilterMessages
(object):
7
def
__init__
(self):
8
rospy.init_node(
'mean_filter'
, anonymous=
True
)
9
10
if
rospy.has_param(
'~nb_occ'
):
11
nb_occurence = rospy.get_param(
'~nb_occ'
)
12
else
:
13
nb_occurence = 5
14
rospy.core.logwarn(
"N number of occurence provided, using 5 as default"
)
15
16
if
rospy.has_param(
'~in_topic'
):
17
input_topic_param = rospy.get_param(
'~in_topic'
)
18
self.
subs
= rospy.Subscriber(input_topic_param, Float64, self.
callback
)
19
self.
pub
= rospy.Publisher(input_topic_param+
"_filtered"
, Float64)
20
else
:
21
rospy.core.logerr(
"No input topic provided, not starting filtering"
)
22
return
23
24
self.
queue
= []
25
self.
queue_len
= nb_occurence
26
self.
mean
= Float64(0.0)
27
28
rospy.spin()
29
30
def
compute_mean
(self):
31
mean = 0.0
32
for
val
in
range(self.
queue_len
):
33
mean = mean+self.
queue
[val].data
34
35
return
mean/self.
queue_len
36
37
def
callback
(self, data):
38
self.queue.append(data)
39
40
if
len(self.
queue
) >= self.
queue_len
:
41
self.
mean
= Float64(self.
compute_mean
())
42
self.queue.pop(0)
43
44
self.pub.publish(self.
mean
)
45
46
47
if
__name__ ==
'__main__'
:
48
FilterMessages
()
sr_utilities.scripts.moving_average_filter.FilterMessages.mean
mean
Definition:
moving_average_filter.py:26
sr_utilities.scripts.moving_average_filter.FilterMessages.subs
subs
Definition:
moving_average_filter.py:18
sr_utilities.scripts.moving_average_filter.FilterMessages.pub
pub
Definition:
moving_average_filter.py:19
sr_utilities.scripts.moving_average_filter.FilterMessages.__init__
def __init__(self)
Definition:
moving_average_filter.py:7
sr_utilities.scripts.moving_average_filter.FilterMessages.compute_mean
def compute_mean(self)
Definition:
moving_average_filter.py:30
sr_utilities.scripts.moving_average_filter.FilterMessages.queue_len
queue_len
Definition:
moving_average_filter.py:25
sr_utilities.scripts.moving_average_filter.FilterMessages
Definition:
moving_average_filter.py:6
sr_utilities.scripts.moving_average_filter.FilterMessages.queue
queue
Definition:
moving_average_filter.py:24
sr_utilities.scripts.moving_average_filter.FilterMessages.callback
def callback(self, data)
Definition:
moving_average_filter.py:37
sr_utilities
Author(s): Ugo Cupcic
autogenerated on Tue Oct 13 2020 03:55:49