calibrate.py
Go to the documentation of this file.
00001 #! /usr/bin/python
00002 #***********************************************************
00003 # Software License Agreement (BSD License)
00004 #
00005 # Copyright (c) 2008, Willow Garage, Inc.
00006 # All rights reserved.
00007 #
00008 # Redistribution and use in source and binary forms, with or without
00009 # modification, are permitted provided that the following conditions
00010 # are met:
00011 #
00012 #  * Redistributions of source code must retain the above copyright
00013 #    notice, this list of conditions and the following disclaimer.
00014 #  * Redistributions in binary form must reproduce the above
00015 #    copyright notice, this list of conditions and the following
00016 #    disclaimer in the documentation and/or other materials provided
00017 #    with the distribution.
00018 #  * Neither the name of the Willow Garage nor the names of its
00019 #    contributors may be used to endorse or promote products derived
00020 #    from this software without specific prior written permission.
00021 #
00022 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033 # POSSIBILITY OF SUCH DAMAGE.
00034 #
00035 
00036 # Author: Wim Meeussen
00037 
00038 from __future__ import with_statement
00039 
00040 import roslib; roslib.load_manifest('turtlebot_calibration')
00041 import yaml
00042 import PyKDL
00043 import rospy
00044 from sensor_msgs.msg import Imu
00045 from nav_msgs.msg import Odometry
00046 from geometry_msgs.msg import Twist
00047 from turtlebot_calibration.msg import ScanAngle
00048 from math import *
00049 import threading
00050 
00051 
00052 def quat_to_angle(quat):
00053     rot = PyKDL.Rotation.Quaternion(quat.x, quat.y, quat.z, quat.w)
00054     return rot.GetRPY()[2]
00055         
00056 def normalize_angle(angle):
00057     res = angle
00058     while res > pi:
00059         res -= 2.0*pi
00060     while res < -pi:
00061         res += 2.0*pi
00062     return res
00063 
00064 
00065 class CalibrateRobot:
00066     def __init__(self):
00067         self.lock = threading.Lock()
00068 
00069         self.has_gyro = rospy.get_param("turtlebot_node/has_gyro")
00070         rospy.loginfo('has_gyro %s'%self.has_gyro)
00071         if self.has_gyro:
00072             self.sub_imu  = rospy.Subscriber('imu', Imu, self.imu_cb)
00073 
00074         self.sub_odom = rospy.Subscriber('odom', Odometry, self.odom_cb)
00075         self.sub_scan = rospy.Subscriber('scan_angle', ScanAngle, self.scan_cb)
00076         self.cmd_pub = rospy.Publisher('cmd_vel', Twist)
00077         self.imu_time = rospy.Time()
00078         self.odom_time = rospy.Time()
00079         self.scan_time = rospy.Time()
00080         
00081         # params
00082         self.inital_wall_angle = rospy.get_param("inital_wall_angle", 0.1)
00083         self.imu_calibrate_time = rospy.get_param("imu_calibrate_time", 10.0)
00084         self.imu_angle = 0
00085         self.imu_time = rospy.Time.now()
00086         self.scan_angle = 0
00087         self.scan_time = rospy.Time.now()
00088         self.odom_angle = 0
00089         self.odom_time = rospy.Time.now()
00090 
00091     def calibrate(self, speed, imu_drift=0):
00092         # rotate 360 degrees
00093         (imu_start_angle, odom_start_angle, scan_start_angle, 
00094          imu_start_time, odom_start_time, scan_start_time) = self.sync_timestamps()
00095         last_angle = odom_start_angle
00096         turn_angle = 0
00097         while turn_angle < 2*pi:
00098             if rospy.is_shutdown():
00099                 return
00100             cmd = Twist()
00101             cmd.angular.z = speed
00102             self.cmd_pub.publish(cmd)
00103             rospy.sleep(0.1)
00104             with self.lock:
00105                 delta_angle = normalize_angle(self.odom_angle - last_angle)
00106             turn_angle += delta_angle
00107             last_angle = self.odom_angle
00108         self.cmd_pub.publish(Twist())
00109 
00110         (imu_end_angle, odom_end_angle, scan_end_angle,
00111          imu_end_time, odom_end_time, scan_end_time) = self.sync_timestamps()
00112 
00113         scan_delta = 2*pi + normalize_angle(scan_end_angle - scan_start_angle)
00114 
00115         odom_delta = 2*pi + normalize_angle(odom_end_angle - odom_start_angle)
00116         rospy.loginfo('Odom error: %f percent'%(100.0*((odom_delta/scan_delta)-1.0)))
00117         
00118         if self.has_gyro:
00119             imu_delta = 2*pi + normalize_angle(imu_end_angle - imu_start_angle) - imu_drift*(imu_end_time - imu_start_time).to_sec()
00120             rospy.loginfo('Imu error: %f percent'%(100.0*((imu_delta/scan_delta)-1.0)))
00121             imu_result = imu_delta/scan_delta
00122         else: 
00123             imu_result = None
00124 
00125         return (imu_result, odom_delta/scan_delta)
00126 
00127 
00128     def imu_drift(self):
00129         if not self.has_gyro:
00130             return 0
00131         # estimate imu drift
00132         rospy.loginfo('Estimating imu drift')
00133         (imu_start_angle, odom_start_angle, scan_start_angle, 
00134          imu_start_time, odom_start_time, scan_start_time) = self.sync_timestamps()
00135         rospy.sleep(self.imu_calibrate_time)
00136         (imu_end_angle, odom_end_angle, scan_end_angle,
00137          imu_end_time, odom_end_time, scan_end_time) = self.sync_timestamps()
00138 
00139         imu_drift = normalize_angle(imu_end_angle - imu_start_angle) / ((imu_end_time - imu_start_time).to_sec())
00140         rospy.loginfo(' ... imu drift is %f degrees per second'%(imu_drift*180.0/pi))
00141         return imu_drift
00142 
00143 
00144     def align(self):
00145         self.sync_timestamps()
00146         rospy.loginfo("Aligning base with wall")
00147         with self.lock:
00148             angle = self.scan_angle
00149         cmd = Twist()
00150 
00151         while angle < -self.inital_wall_angle or angle > self.inital_wall_angle:
00152             if rospy.is_shutdown():
00153                 exit(0)
00154             if angle > 0:
00155                 cmd.angular.z = -0.3
00156             else:
00157                 cmd.angular.z = 0.3
00158             self.cmd_pub.publish(cmd)
00159             rospy.sleep(0.05)
00160             with self.lock:
00161                 angle = self.scan_angle
00162 
00163 
00164 
00165     def sync_timestamps(self, start_time=None):
00166         if not start_time:
00167             start_time = rospy.Time.now() + rospy.Duration(0.5)
00168         while not rospy.is_shutdown():
00169             rospy.sleep(0.3)
00170             with self.lock:
00171                 if self.imu_time < start_time and self.has_gyro:
00172                     rospy.loginfo("Still waiting for imu")
00173                 elif self.odom_time < start_time:
00174                     rospy.loginfo("Still waiting for odom")
00175                 elif self.scan_time < start_time:
00176                     rospy.loginfo("Still waiting for scan")
00177                 else:
00178                     return (self.imu_angle, self.odom_angle, self.scan_angle,
00179                             self.imu_time, self.odom_time, self.scan_time)
00180         exit(0)
00181         
00182 
00183     def imu_cb(self, msg):
00184         with self.lock:
00185             angle = quat_to_angle(msg.orientation)
00186             self.imu_angle = angle
00187             self.imu_time = msg.header.stamp
00188 
00189     def odom_cb(self, msg):
00190         with self.lock:
00191             angle = quat_to_angle(msg.pose.pose.orientation)
00192             self.odom_angle = angle
00193             self.odom_time = msg.header.stamp
00194 
00195     def scan_cb(self, msg):
00196         with self.lock:
00197             angle = msg.scan_angle
00198             self.scan_angle = angle
00199             self.scan_time = msg.header.stamp
00200 
00201 
00202 def writeParamsToLaunchFile(gyro, odom):
00203     try:
00204         f = open("/etc/ros/distro/turtlebot.launch", "r")
00205         # this is totally NOT the best way to solve this problem.
00206         foo = []
00207         for lines in f:
00208             if "turtlebot_node/gyro_scale_correction" in lines:
00209                 foo.append("  <param name=\"turtlebot_node/gyro_scale_correction\" value=\"%f\"/>\n" % gyro)
00210             elif "turtlebot_node/odom_angular_scale_correction" in lines:
00211                 foo.append("  <param name=\"turtlebot_node/odom_angular_scale_correction\" value=\"%f\"/>\n" % odom)
00212             else:
00213                 foo.append(lines)
00214         f.close()
00215 
00216         # and... write!
00217         f = open("/etc/ros/distro/turtlebot.launch", "w")
00218         for i in foo:
00219             f.write(i)
00220         f.close()
00221         rospy.loginfo("Automatically updated turtlebot.launch, please restart the turtlebot service.")
00222     except:
00223         rospy.loginfo("Could not automatically update turtlebot.launch, please manually update it.")
00224 
00225 
00226 def main():
00227     rospy.init_node('scan_to_angle')
00228     robot = CalibrateRobot()
00229     
00230     imu_drift = robot.imu_drift()
00231     imu_corr = []
00232     odom_corr = []
00233     for speed in (0.3, 0.7, 1.0, 1.5):
00234         robot.align()
00235         (imu, odom) = robot.calibrate(speed, imu_drift)
00236         if imu:
00237             imu_corr.append(imu)
00238         odom_corr.append(odom)
00239 
00240     if len(imu_corr)>0:    
00241         imu_res = 1.0/(sum(imu_corr)/len(imu_corr))
00242         rospy.loginfo("Multiply the 'turtlebot_node/gyro_scale_correction' parameter with %f"%imu_res)
00243 
00244     odom_res = 1.0/(sum(odom_corr)/len(odom_corr))
00245     rospy.loginfo("Multiply the 'turtlebot_node/odom_angular_scale_correction' parameter with %f"%odom_res)
00246     writeParamsToLaunchFile(imu_res, odom_res)
00247 
00248 
00249 if __name__ == '__main__':
00250     main()


turtlebot_calibration
Author(s): Wim Meeussen
autogenerated on Mon Oct 6 2014 08:07:14