Loading files
This file is pinned to the selected release. Download the original to open it in your design tool.
25e0ce228This file is pinned to the selected release. Download the original to open it in your design tool.
import numpy as np
import rospy
from std_msgs.msg import Float32
class FloatHandler(object):
"""
Handler for ROS topics of type: std_msgs/Float32
Args:
topic_name: Name of ROS topic to be subscribed
buffer_size: Variable buffer, depend on frame rate of topic, default: 500
queue_size: Subscriber queue_size
"""
def __init__(self, topic_name, buffer_size=500, queue_size=10):
self.data_msg = Float32()
self.data = 0.0
self.topic_float = topic_name
self.queue_size = queue_size
self.buffer_size = buffer_size
self.counter = 0
self.buffer = np.zeros([self.buffer_size, 1])
self.sub = rospy.Subscriber(self.topic_float, Float32, self.callback, queue_size=self.queue_size)
def callback(self, msg):
self.data_msg = msg
if self.counter < self.buffer_size:
self.buffer[self.counter] = [self.data_msg.data]
else:
rospy.loginfo("FloatHandler for: " + self.topic_float + " has reach buffer size.")
self.counter += 1
def get_value(self):
if self.counter > 0:
self.data = np.sum(self.buffer[:, 0]) / self.counter
self.buffer = np.zeros([self.buffer_size, 1])
self.counter = 0
return self.data