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 @@
rospy
sensor_msgs
std_msgs
+ geometry_msgs
rospy
sensor_msgs
std_msgs
+ geometry_msgs
rospy
sensor_msgs
std_msgs
+ geometry_msgs
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()