imu_covar_fix.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 '''
00003 Created on 18.10.2012
00004 
00005 @author: Martin Guenther <mguenthe@uos.de>
00006 
00007 This ROS node changes the covariance inside an IMU message. This is necessary
00008 for IMU nodes that report a wrong covariance (like android_sensors_driver).
00009 '''
00010 
00011 import roslib
00012 roslib.load_manifest('imu_recalibration')
00013 
00014 import rospy
00015 
00016 from sensor_msgs.msg import Imu
00017 
00018 pub = None
00019 
00020 def callback(msg_in):
00021     msg_out = msg_in
00022     msg_out.orientation_covariance = [2.0e-7, 0.0, 0.0, 0.0, 2.0e-7, 0.0, 0.0, 0.0, 2.0e-7]
00023     pub.publish(msg_out)
00024 
00025 def main():
00026     global pub
00027     rospy.init_node('imu_covar_fix', anonymous=True)
00028     pub = rospy.Publisher('imu_covar_fixed', Imu)
00029     rospy.Subscriber('imu', Imu, callback)
00030     rospy.spin()
00031 
00032 if __name__ == '__main__':
00033     try:
00034         main()
00035     except rospy.ROSInterruptException:
00036         pass
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends


imu_recalibration
Author(s): Jochen Sprickerhof, Martin Günther
autogenerated on Tue May 28 2013 15:07:56