imu2dcalibrator.py
Go to the documentation of this file.
```00001 #!/usr/bin/env python
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:
00053     self.values = values
00055
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
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
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:
00096
00097     f = os.popen('/home/pi/catkin_ws/devel/lib/evarobot_minimu9/./evarobot_minimu9 2000 > calib.txt')
00099     fo = open("calib.txt", "r+")
00101     fo.close()
00102     f = os.popen('rm calib.txt')
00103
00105     print(" done.", file=sys.stderr)
00106
00107     # Stop the robot.
00108     twist.angular.z = 0.0;
00109     pub.publish(twist)
00110
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
00119     if verbose:
00120       print("Initial calibration: " + cal1.info_string(), file=sys.stderr)
00121
00122     cal_values = scipy.optimize.fmin(negative_score, cal1.values, \
00124                                      full_output=False, disp=verbose, retall=False)
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
00136
00137   except UserError as e:
00138     print("Error: " + str(e), file=sys.stderr)
00139
00140   except KeyboardInterrupt:
00141     print("", file=sys.stderr)   # newline to tidy things up
00142     pass
00143
00145   vectors = [Vector(*[int(s) for s in line.split()[0:2]]) for line in file]
00146   return vectors
00147
00149   guess = []
00150   for axis in Axes:
00151     values = [v[axis] for v in readings]
00152     guess.extend(percentile_to_value(values, 1, 99))
00154
00156   warn = False
00157   for axis in Axes:
00158     readings = [v[axis] for v in raw_vectors]
00161     min_cal = cal.values[axis * 2]
00162     max_cal = cal.values[axis * 2 + 1]
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()
```

evarobot_start
Author(s): Mehmet Akcakoca
autogenerated on Fri Feb 12 2016 01:15:32