From bf9f492d27460487bb55523c8c95273dfb9dd25b Mon Sep 17 00:00:00 2001
From: Renukanandan Tumu <renukanandan.tumu@uconn.edu>
Date: Fri, 9 Feb 2018 16:41:34 -0500
Subject: [PATCH] Now publishing cleaned values

---
 src/imu_cleaning.py | 22 ++++++++++++++++++++++
 1 file changed, 22 insertions(+)

diff --git a/src/imu_cleaning.py b/src/imu_cleaning.py
index 565553c..64757af 100755
--- a/src/imu_cleaning.py
+++ b/src/imu_cleaning.py
@@ -2,9 +2,29 @@
 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")
@@ -17,6 +37,8 @@ def main():
 	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()