From db0e192371a90aa1b1201430896337b49a1da2f8 Mon Sep 17 00:00:00 2001
From: Renukanandan Tumu <renukanandan.tumu@uconn.edu>
Date: Tue, 6 Feb 2018 18:35:39 -0500
Subject: [PATCH] Added gravity calculation functionality.

- Average of first 50 records seen is used to calculate gravity.
---
 CMakeLists.txt      |  1 +
 package.xml         |  3 +++
 src/imu_cleaning.py | 22 ++++++++++++++++++++++
 3 files changed, 26 insertions(+)
 create mode 100755 src/imu_cleaning.py

diff --git a/CMakeLists.txt b/CMakeLists.txt
index ff8acdb..30e665c 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -11,6 +11,7 @@ find_package(catkin REQUIRED COMPONENTS
   rospy
   sensor_msgs
   std_msgs
+  geometry_msgs
 )
 
 ## System dependencies are found with CMake's conventions
diff --git a/package.xml b/package.xml
index 5831b36..d74073b 100644
--- a/package.xml
+++ b/package.xml
@@ -52,12 +52,15 @@
   <build_depend>rospy</build_depend>
   <build_depend>sensor_msgs</build_depend>
   <build_depend>std_msgs</build_depend>
+  <build_depend>geometry_msgs</build_depend>
   <build_export_depend>rospy</build_export_depend>
   <build_export_depend>sensor_msgs</build_export_depend>
   <build_export_depend>std_msgs</build_export_depend>
+  <build_export_depend>geometry_msgs</build_export_depend>
   <exec_depend>rospy</exec_depend>
   <exec_depend>sensor_msgs</exec_depend>
   <exec_depend>std_msgs</exec_depend>
+  <exec_depend>geometry_msgs</exec_depend>
 
 
   <!-- The export tag contains other, unspecified, tags -->
diff --git a/src/imu_cleaning.py b/src/imu_cleaning.py
new file mode 100755
index 0000000..565553c
--- /dev/null
+++ b/src/imu_cleaning.py
@@ -0,0 +1,22 @@
+#!/usr/bin/env python
+import rospy
+from geometry_msgs.msg import Quaternion, Vector3
+from sensor_msgs.msg import Imu
+
+import math
+
+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)))
+
+if __name__=='__main__':
+	main()