imu_cleaning.py

#!/usr/bin/env python
import rospy
from geometry_msgs.msg import Quaternion, Vector3
from sensor_msgs.msg import Imu
from std_msgs.msg import *

import math

class VariableMaintainer():
	def __init__(self):
		self._time = rospy.Time.now()
	
	@property
	def time(self):
		return self._time

	@time.setter
	def time(self, value):
		self._time = value

def publishCleanImu(msg, correctionVector):
	pub = rospy.Publisher('clean_imu',Imu)
	msg.linear_acceleration.x -= correctionVector[0]
	msg.linear_acceleration.y -= correctionVector[1]
	msg.linear_acceleration.y -= correctionVector[2]
	pub.publish(msg)

def main():
	# Collect first 50 entries and average them.
	rospy.init_node("imu_cleaner")
	totals =[0,0,0] 
	for i in range(50):
		cval = rospy.client.wait_for_message('imu',Imu)
		totals[0]+=cval.linear_acceleration.x
		totals[1]+=cval.linear_acceleration.y
		totals[2]+=cval.linear_acceleration.z
	averages = map(lambda x: x/50,totals)
	print(averages)
	print("Magnitude is:", math.sqrt(math.pow(averages[0],2)+math.pow(averages[1],2)+math.pow(averages[2],2)))
	rospy.Subscriber('imu', Imu, publishCleanImu, averages)
	rospy.spin()

if __name__=='__main__':
	main()