class GPSAltitudeMovingAverage():
# Node initialization
def __init__(self):
window_size = rospy.get_param("/gps_altitude_ma/gps_window_size", 4)
self.ma = MovingAverage(window_size)
# Create the publisher and subscriber
self.pub = rospy.Publisher('/uav/sensors/gps_altitude_ma',
AltitudeStamped,
queue_size=1)
self.sub = rospy.Subscriber('/uav/sensors/gps',
PoseStamped,
self.process_altitude,
queue_size=1)
rospy.spin()
def process_altitude(self, msg):
# add new value to the moving average list
# use MovingAverage to calculate the new moving average
# use the same timestamp as in msg
# publish
self.ma.add(msg.pose.position.z)
avg = self.ma.get_average()
altStamped = AltitudeStamped()
altStamped.value = avg
altStamped.stamp = msg.header.stamp
self.pub.publish(altStamped)
class LIDARAltitudeMA:
# Node initialization
def __init__(self):
# Create the publisher and subscriber
self.moving_average = MovingAverage(5)
self.pub = rospy.Publisher('/uav/sensors/lidar_altitude_ma',
AltitudeStamped,
queue_size=1)
self.sub = rospy.Subscriber('/uav/sensors/lidar_position',
PointStamped,
self.process_altitude,
queue_size=1)
self.lidar_error = float(
rospy.get_param('/uav/sensors/lidar_position_noise', str(0)))
self.altitude = AltitudeStamped()
self.lidar_point = PointStamped()
rospy.spin()
def process_altitude(self, msg):
self.lidar_point = msg
self.moving_average.add(self.lidar_point.point.z)
self.altitude.value = self.moving_average.get_average()
self.altitude.stamp = self.lidar_point.header.stamp
self.altitude.error = self.lidar_error / math.sqrt(
self.moving_average.window_size)
self.pub.publish(self.altitude)
def test_window(self):
mw = MovingAverage(5)
values = [random() for i in range(10)]
for v in values:
mw.add(v)
self.assertTrue(
abs(mw.get_average() - np.average(values[5:]) <= 0.0001))
def test_window_changing(self):
mw = MovingAverage(5)
zero_values = [random() for i in range(5)]
for v in zero_values:
mw.add(v)
self.assertTrue(
abs(mw.get_average() - np.average(zero_values) <= 0.0001))
five_values = [5 + random() for i in range(5)]
for v in five_values:
mw.add(v)
self.assertTrue(
abs(mw.get_average() - np.average(five_values) <= 5.0001))
