imu_recalibration.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 '''
00003 Created on 16.10.2012
00004 
00005 @author: Jochen Sprickerhof <jochen@sprickerhof.de> and Martin Guenther <mguenthe@uos.de>
00006 '''
00007 
00008 import roslib; roslib.load_manifest('imu_recalibration')
00009 import rospy
00010 import tf
00011 from sensor_msgs.msg import Imu
00012 from nav_msgs.msg import Odometry
00013 from math import pi
00014 
00015 class ImuRecalibration:
00016     """
00017     This ROS node recalibrates the IMU (i.e., calculates the IMU drift and
00018     subtracts it) whenever the robot is standing.
00019     """
00020 
00021     # number of IMU messages to average over
00022     NUM_SAMPLES = 100
00023 
00024     # drifts larger than this value are ignored (rad/s)
00025     # (the maximum drift actually measured is 2pi in 120 seconds)
00026     MAX_DELTA = 2.0 * pi / 120.0 / NUM_SAMPLES
00027 
00028     # the robot is considered to be moving if the angular velocity is larger than this (rad/s)
00029     MAX_ANGULAR_VEL = 2.0 * pi / 3600.0
00030 
00031     def __init__(self):
00032         rospy.init_node('imu_recalibration')
00033         rospy.Subscriber("imu", Imu, self.imu_callback)
00034         rospy.Subscriber("odom", Odometry, self.odom_callback)
00035         self.pub = rospy.Publisher('imu_recalibrated', Imu)
00036         self.error = 0.0
00037         self.delta = 0.0
00038         self.delta_new = 0.0
00039         self.yaw_old = 0.0
00040         self.calibration_counter = -1
00041         #self.debug_file = open('/tmp/imu_recalibration.txt', 'w')
00042         rospy.loginfo('imu_recalibration node initialized.')
00043 
00044     @classmethod
00045     def normalize(cls, angle):
00046         while angle < pi:
00047             angle += 2.0 * pi
00048         while angle > pi:
00049             angle -= 2.0 * pi
00050         return angle
00051 
00052     def imu_callback(self, msg_in):
00053         (r, p, yaw) = tf.transformations.euler_from_quaternion([msg_in.orientation.x,
00054             msg_in.orientation.y, msg_in.orientation.z, msg_in.orientation.w])
00055 
00056         self.calibration_counter += 1
00057         if self.calibration_counter == 0:
00058             # start of calibration interval --> reset
00059             self.delta_new = 0.0
00060         else:
00061             # inside calibration interval --> accumulate
00062             self.delta_new += self.normalize(yaw - self.yaw_old)
00063 
00064         if self.calibration_counter == ImuRecalibration.NUM_SAMPLES:
00065             self.delta_new /= ImuRecalibration.NUM_SAMPLES
00066             # end of calibration interval
00067             if abs(self.delta_new) < ImuRecalibration.MAX_DELTA:
00068                 self.delta = self.delta_new
00069                 rospy.loginfo("New IMU delta: %f" % self.delta)
00070             else:
00071                 # this can happen if the base was switched off and on again
00072                 rospy.logwarn("IMU delta too large, ignoring: %f" % self.delta_new)
00073 
00074             self.calibration_counter = -1
00075 
00076         self.error += self.delta;
00077         self.error = self.normalize(self.error)
00078         self.yaw_old = yaw
00079         yaw_new = self.normalize(yaw - self.error)
00080 
00081         q = tf.transformations.quaternion_from_euler(r, p, yaw_new)
00082         msg_out = msg_in
00083         msg_out.orientation.x = q[0]
00084         msg_out.orientation.y = q[1]
00085         msg_out.orientation.z = q[2]
00086         msg_out.orientation.w = q[3]
00087 
00088         #self.debug_file.write("%s %s %s %s %s %s\n" % (yaw, yaw_new, self.error, self.delta, self.delta_new, self.calibration_counter))
00089         self.pub.publish(msg_out)
00090 
00091     def odom_callback(self, msg):
00092         if abs(msg.twist.twist.angular.z) > ImuRecalibration.MAX_ANGULAR_VEL:
00093             rospy.loginfo("Resetting imu_recalibration (robot is moving)")
00094             self.calibration_counter = -1
00095 
00096 if __name__ == '__main__':
00097     ImuRecalibration()
00098     try:
00099         rospy.spin()
00100     except rospy.ROSInterruptException:
00101         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