calibrate.py
Go to the documentation of this file.
00001 #!/usr/bin/python
00002 # Software License Agreement (BSD License)
00003 #
00004 # Copyright (c) 2008, Willow Garage, Inc.
00005 # All rights reserved.
00006 #
00007 # Redistribution and use in source and binary forms, with or without
00008 # modification, are permitted provided that the following conditions
00009 # are met:
00010 #
00011 #  * Redistributions of source code must retain the above copyright
00012 #    notice, this list of conditions and the following disclaimer.
00013 #  * Redistributions in binary form must reproduce the above
00014 #    copyright notice, this list of conditions and the following
00015 #    disclaimer in the documentation and/or other materials provided
00016 #    with the distribution.
00017 #  * Neither the name of the Willow Garage nor the names of its
00018 #    contributors may be used to endorse or promote products derived
00019 #    from this software without specific prior written permission.
00020 #
00021 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 # POSSIBILITY OF SUCH DAMAGE.
00033 #
00034 
00035 # Author: Stuart Glaser
00036 
00037 from __future__ import with_statement
00038 
00039 import roslib
00040 import copy
00041 import threading
00042 import sys, os
00043 from time import sleep
00044 
00045 # Loads interface with the robot.
00046 roslib.load_manifest('pr2_bringup')
00047 import rospy
00048 from std_msgs.msg import *
00049 from pr2_mechanism_msgs.srv import LoadController, UnloadController, SwitchController, SwitchControllerRequest
00050 from std_msgs.msg import Bool
00051 
00052 load_controller = rospy.ServiceProxy('pr2_controller_manager/load_controller', LoadController)
00053 unload_controller = rospy.ServiceProxy('pr2_controller_manager/unload_controller', UnloadController)
00054 switch_controller = rospy.ServiceProxy('pr2_controller_manager/switch_controller', SwitchController)
00055 
00056 def calibrate(controllers):
00057 
00058     success = True
00059 
00060     if type(controllers) is not list:
00061         controllers = [controllers]
00062 
00063     launched = []
00064     try:
00065         # Loads the controllers
00066         for c in controllers:
00067             resp = load_controller(c)
00068             if resp.ok == 0:
00069                 rospy.logerr("Failed: %s" % c)
00070                 success = False
00071             else:
00072                 launched.append(c)
00073         print "Launched: %s" % ', '.join(launched)
00074 
00075         # Starts the launched controllers
00076         switch_controller(launched, [], SwitchControllerRequest.BEST_EFFORT)
00077 
00078         # Sets up callbacks for calibration completion
00079         waiting_for = launched[:]
00080         def calibrated(msg, name):  # Somewhat not thread-safe
00081             if name in waiting_for:
00082                 waiting_for.remove(name)
00083         for name in waiting_for:
00084             rospy.Subscriber("%s/calibrated" % name, Empty, calibrated, name)
00085 
00086         # Waits until all the controllers have calibrated
00087         while waiting_for and not rospy.is_shutdown():
00088             print "Waiting for: %s" % ', '.join(waiting_for)
00089             sleep(0.5)
00090     finally:
00091         for name in launched:
00092             try:
00093                 resp_stop = switch_controller([], [name], SwitchControllerRequest.STRICT)
00094                 if (resp_stop == 0):
00095                     rospy.logerr("Failed to stop controller %s" % name)
00096                 resp_unload = unload_controller(name)
00097                 if (resp_unload == 0):
00098                     rospy.logerr("Failed to unload controller %s" % name)
00099             except Exception, ex:
00100                 rospy.logerr("Failed to stop/unload controller %s" % name)
00101     return success
00102 
00103 def calibrate_imu():
00104     class is_calibrated_helper:
00105         def __init__(self):
00106             self.is_calibrated = False
00107             self.cond = threading.Condition()
00108 
00109         def callback(self, msg):
00110             if msg.data:
00111                 with self.cond:
00112                     self.is_calibrated = True
00113                     self.cond.notify()
00114 
00115         def wait_for_calibrated(self, topic, timeout):
00116             self.sub = rospy.Subscriber(topic,Bool,self.callback)
00117             try:
00118                 with self.cond:
00119                     if not self.is_calibrated:
00120                         self.cond.wait(timeout)
00121                 return self.is_calibrated
00122             finally:
00123                 self.sub.unregister()
00124             return self.is_calibrated
00125 
00126     print "Waiting up to 20s for IMU calibration to complete."
00127     helper = is_calibrated_helper()
00128     if not helper.wait_for_calibrated("torso_lift_imu/is_calibrated", 20):
00129         rospy.logerr("IMU took too long to calibrate.")
00130         return False
00131     return True
00132 
00133 def main():
00134     pub_calibrated = rospy.Publisher('calibrated', Bool, latch=True)
00135     rospy.wait_for_service('pr2_controller_manager/load_controller')
00136     rospy.wait_for_service('pr2_controller_manager/switch_controller')
00137     rospy.wait_for_service('pr2_controller_manager/unload_controller')
00138     if  rospy.is_shutdown(): return
00139 
00140     rospy.init_node('calibration', anonymous=True)
00141     pub_calibrated.publish(False)
00142 
00143     # Don't calibrate the IMU unless ordered to by user
00144     cal_imu = rospy.get_param('calibrate_imu', False)
00145 
00146     if cal_imu:
00147         imustatus = calibrate_imu()
00148     else: 
00149         imustatus = True
00150 
00151     xml = ''
00152 
00153     controllers = rospy.myargv()[1:]
00154 
00155     if not calibrate(controllers):
00156         sys.exit(3)
00157 
00158     pub_calibrated.publish(True)
00159 
00160     if not imustatus:
00161         print "Mechanism calibration complete, but IMU calibration failed."
00162     else:
00163         print "Calibration complete"
00164 
00165     rospy.spin()
00166 
00167 if __name__ == '__main__':
00168     main()


pr2_bringup
Author(s): Wim Meeussen
autogenerated on Sat Nov 30 2013 21:19:51