Skip to content

Commit

Permalink
Browse files Browse the repository at this point in the history
Added gravity calculation functionality.
- Average of first 50 records seen is used to calculate gravity.
  • Loading branch information
ret15108 committed Feb 6, 2018
1 parent 37d6e07 commit db0e192
Show file tree
Hide file tree
Showing 3 changed files with 26 additions and 0 deletions.
1 change: 1 addition & 0 deletions CMakeLists.txt
Expand Up @@ -11,6 +11,7 @@ find_package(catkin REQUIRED COMPONENTS
rospy
sensor_msgs
std_msgs
geometry_msgs
)

## System dependencies are found with CMake's conventions
Expand Down
3 changes: 3 additions & 0 deletions package.xml
Expand Up @@ -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 -->
Expand Down
22 changes: 22 additions & 0 deletions 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()

0 comments on commit db0e192

Please sign in to comment.