00001
00002
00003 from __future__ import print_function
00004 import sys
00005 import math
00006 import scipy.optimize
00007 import os
00008
00009
00010 import rospy
00011 from geometry_msgs.msg import Twist
00012
00013 verbose = False
00014
00015 class UserError(Exception):
00016 pass
00017
00018 def average(list):
00019 return sum(list)/len(list)
00020
00021 def percentile_to_value(list, *percentiles):
00022 list = sorted(list)
00023 return [list[int(p / 100.0 * (len(list)-1))] for p in percentiles]
00024
00025 def variance(list):
00026 m = average(list)
00027 return sum([(i-m)**2 for i in list]) / float(len(list))
00028
00029 def std_deviation(list):
00030 return math.sqrt(variance(list))
00031
00032 class Vector(object):
00033 def __init__(self, x, y):
00034 self.x, self.y = x, y
00035 def __str__(self):
00036 return "(" + str(self.x) + ", " + str(self.y) + ")"
00037
00038 def __getitem__(self, key):
00039 if key == 0:
00040 return self.x
00041 elif key == 1:
00042 return self.y
00043 else:
00044 raise Exception("Invalid key")
00045
00046 def magnitude(self):
00047 return math.sqrt(self.x**2 + self.y**2)
00048
00049 Axes = range(2)
00050
00051 class Calibration:
00052 def __init__(self, values, raw_readings=None):
00053 self.values = values
00054 self.raw_readings = raw_readings
00055
00056 def scale(self, raw_reading):
00057 return Vector((raw_reading[0] - self.values[0])/float(self.values[1] - self.values[0]) * 2 - 1,
00058 (raw_reading[1] - self.values[2])/float(self.values[3] - self.values[2]) * 2 - 1)
00059
00060 def __str__(self):
00061 return "%d %d %d %d -1900 1601" % tuple(self.values)
00062
00063 def info_string(self):
00064 return "%-32s avg=%7.4f stdev=%7.4f score=%7.4f" % (
00065 str(self),
00066 average(self.scaled_magnitudes()),
00067 std_deviation(self.scaled_magnitudes()),
00068 self.score()
00069 )
00070
00071 def scaled_magnitudes(self):
00072 return [s.magnitude() for s in self.scaled_readings()]
00073
00074 def scaled_readings(self):
00075 return [self.scale(r) for r in self.raw_readings]
00076
00077 def score(self):
00078 return -average([(m - 1.0)**2 for m in self.scaled_magnitudes()])
00079
00080 def negative_score(values, raw_readings):
00081 cal = Calibration(values, raw_readings)
00082 return -cal.score()
00083
00084 def run():
00085 pub = rospy.Publisher('cmd_vel', Twist, queue_size=10, latch=True)
00086 rospy.init_node('imu2calibrator', anonymous=True)
00087 twist = Twist()
00088 twist.linear.x = 0;
00089 twist.linear.y = 0; twist.linear.z = 0;
00090 twist.angular.x = 0; twist.angular.y = 0;
00091 twist.angular.z = 0.7;
00092 pub.publish(twist)
00093
00094 try:
00095 print("Reading data...", end="", file=sys.stderr)
00096
00097 f = os.popen('/home/pi/catkin_ws/devel/lib/evarobot_minimu9/./evarobot_minimu9 2000 > calib.txt')
00098 fread = f.read()
00099 fo = open("calib.txt", "r+")
00100 file = fo.readlines();
00101 fo.close()
00102 f = os.popen('rm calib.txt')
00103
00104 raw_readings = read_vectors(file)
00105 print(" done.", file=sys.stderr)
00106
00107
00108 twist.angular.z = 0.0;
00109 pub.publish(twist)
00110
00111 if len(raw_readings) < 300:
00112 print("Warning: Only " + str(len(raw_readings)) + " readings were provided.", file=sys.stderr)
00113
00114 print("Optimizing calibration...", end="", file=sys.stderr)
00115 if verbose:
00116 print("", file=sys.stderr)
00117
00118 cal1 = guess(raw_readings)
00119 if verbose:
00120 print("Initial calibration: " + cal1.info_string(), file=sys.stderr)
00121
00122 cal_values = scipy.optimize.fmin(negative_score, cal1.values, \
00123 args=[raw_readings], maxiter=1000, \
00124 full_output=False, disp=verbose, retall=False)
00125 cal = Calibration(cal_values, raw_readings)
00126 print(" done.", file=sys.stderr)
00127
00128 if verbose:
00129 print("Final calibration: " + cal.info_string())
00130
00131 fo = open(os.path.expanduser('~/.minimu9-ahrs-cal'), "w")
00132 fo.write(str(cal));
00133 fo.close()
00134
00135 warn_about_bad_calibration(cal, raw_readings)
00136
00137 except UserError as e:
00138 print("Error: " + str(e), file=sys.stderr)
00139
00140 except KeyboardInterrupt:
00141 print("", file=sys.stderr)
00142 pass
00143
00144 def read_vectors(file):
00145 vectors = [Vector(*[int(s) for s in line.split()[0:2]]) for line in file]
00146 return vectors
00147
00148 def guess(readings):
00149 guess = []
00150 for axis in Axes:
00151 values = [v[axis] for v in readings]
00152 guess.extend(percentile_to_value(values, 1, 99))
00153 return Calibration(guess, readings)
00154
00155 def warn_about_bad_calibration(cal, raw_vectors):
00156 warn = False
00157 for axis in Axes:
00158 readings = [v[axis] for v in raw_vectors]
00159 min_reading = min(readings)
00160 max_reading = max(readings)
00161 min_cal = cal.values[axis * 2]
00162 max_cal = cal.values[axis * 2 + 1]
00163 if min_cal < min_reading - (max_reading - min_reading) or \
00164 max_cal > max_reading + (max_reading - min_reading):
00165 warn = True
00166 break
00167 if warn:
00168 print("Warning: The generated calibration appears to be wrong; " +
00169 "please try again with a different data set.", file=sys.stderr)
00170
00171 if __name__=='__main__':
00172 run()