calibrate_pr2.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: Wim Meeussen
00036 # Calibrates the PR-2 in a safe sequence
00037 
00038 
00039 import roslib
00040 roslib.load_manifest('pr2_bringup')
00041 import rospy
00042 import getopt
00043 import yaml
00044 import sys
00045 from std_msgs.msg import *
00046 from pr2_mechanism_msgs.srv import LoadController, UnloadController, SwitchController, SwitchControllerRequest, ListControllers
00047 from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus, KeyValue
00048 from pr2_controllers_msgs.srv import QueryCalibrationState
00049 from std_msgs.msg import Bool
00050 from std_srvs.srv import Empty
00051 from sensor_msgs.msg import *
00052 
00053 
00054 
00055 calibration_params_namespace = "calibration_controllers"
00056 load_controller = rospy.ServiceProxy('pr2_controller_manager/load_controller', LoadController)
00057 unload_controller = rospy.ServiceProxy('pr2_controller_manager/unload_controller', UnloadController)
00058 switch_controller = rospy.ServiceProxy('pr2_controller_manager/switch_controller', SwitchController)
00059 list_controllers = rospy.ServiceProxy('pr2_controller_manager/list_controllers', ListControllers)
00060 
00061 hold_position = {'r_shoulder_pan': -0.7, 'l_shoulder_pan': 0.7, 'r_elbow_flex': -2.0, 
00062                  'l_elbow_flex': -2.0, 'r_upper_arm_roll': 0.0, 'l_upper_arm_roll': 0.0, 
00063                  'r_shoulder_lift': 1.0, 'l_shoulder_lift': 1.0}
00064 
00065 force_calibration = False
00066 
00067 def get_controller_name(joint_name):
00068     return calibration_params_namespace+"/calibrate/cal_%s" % joint_name
00069 
00070 def set_force_calibration(joint_name):
00071     rospy.set_param(calibration_params_namespace+"/calibrate/cal_%s/force_calibration" % joint_name, True)
00072 
00073 def get_holding_name(joint_name):
00074     return "%s/hold/%s_position_controller" % (calibration_params_namespace, joint_name)
00075 
00076 def get_service_name(joint_name):
00077     return '%s/is_calibrated'%get_controller_name(joint_name)
00078 
00079 global last_joint_states
00080 last_joint_states = None
00081 def joint_states_cb(msg):
00082     global last_joint_states
00083     last_joint_states = msg
00084 rospy.Subscriber('joint_states', JointState, joint_states_cb)
00085     
00086 global motors_halted
00087 motors_halted = None
00088 def motor_state_cb(msg):
00089     global motors_halted
00090     motors_halted = msg.data
00091     rospy.logdebug("motors halted = %d"%motors_halted)
00092 rospy.Subscriber('pr2_etherCAT/motors_halted', Bool, motor_state_cb)
00093 
00094 pub_diag = rospy.Publisher('/diagnostics', DiagnosticArray) 
00095 def diagnostics(level, msg_short, msg_long):
00096     if level == 0:
00097         rospy.loginfo(msg_long)        
00098     elif level == 1:
00099         rospy.logwarn(msg_long)
00100     elif level == 2:
00101         rospy.logerr(msg_long)
00102     d = DiagnosticArray() 
00103     d.header.stamp = rospy.Time.now() 
00104     ds = DiagnosticStatus() 
00105     ds.level = level
00106     ds.message = msg_long
00107     ds.name = msg_short
00108     d.status = [ds] 
00109     pub_diag.publish(d) 
00110 
00111 
00112 
00113 class HoldingController:
00114     def __init__(self, joint_name):
00115         self.joint_name = joint_name
00116         rospy.logdebug("Loading holding controller: %s" %get_holding_name(joint_name))
00117         resp = load_controller(get_holding_name(joint_name)) 
00118         if resp.ok:
00119             rospy.logdebug("Starting holding controller for joint %s."%joint_name)
00120             switch_controller([get_holding_name(joint_name)], [], SwitchControllerRequest.STRICT)
00121             self.pub_cmd = rospy.Publisher("%s/command" %get_holding_name(joint_name), Float64, latch=True)
00122         else:
00123             rospy.logerr("Failed to load holding controller for joint %s."%joint_name)
00124             raise Exception('Failure to load holding controller')
00125                                   
00126     def __del__(self):
00127         switch_controller([], [get_holding_name(self.joint_name)], SwitchControllerRequest.STRICT)
00128         unload_controller(get_holding_name(self.joint_name))
00129         self.pub_cmd.unregister()
00130                                
00131     def hold(self, position):
00132         self.pub_cmd.publish(Float64(position))
00133 
00134 
00135 
00136 class StatusPub:
00137     def __init__(self):
00138         self.pub_status = rospy.Publisher('calibration_status', String)
00139         self.status = {}
00140         self.status['active'] = []
00141         self.status['done'] = []
00142         
00143     def publish(self, active=None):
00144         if active:
00145             self.status['active'] = active
00146         else:
00147             self.status['done'].extend(self.status['active'])
00148             self.status['active'] = []
00149             
00150         str = "====\n"
00151         str += "Calibrating: %s\n"%", ".join(self.status["active"])
00152         str += "Calibrated: %s\n"%", ".join(self.status["done"])
00153         self.pub_status.publish(str)
00154 
00155 
00156 
00157 class CalibrateParallel:
00158     def __init__(self, joints, status):
00159         self.joints = []
00160         self.hold_controllers = []
00161         self.services = {}
00162         self.status = status
00163 
00164         # spawn calibration controllers for all joints
00165         for j in joints:
00166             rospy.logdebug("Loading controller: %s" %get_controller_name(j))
00167             global force_calibration
00168             if force_calibration:
00169                 set_force_calibration(j)
00170             resp = load_controller(get_controller_name(j)) 
00171             if resp.ok:
00172                 # get service call to calibration controller to check calibration state
00173                 rospy.logdebug("Waiting for service: %s" %get_service_name(j))
00174                 rospy.wait_for_service(get_service_name(j))
00175                 self.services[j] = rospy.ServiceProxy(get_service_name(j), QueryCalibrationState)
00176                 self.joints.append(j)
00177             else:
00178                 rospy.logerr("Failed to load calibration for joint %s. Skipping this joint"%j)
00179 
00180 
00181     def __del__(self):
00182         # stop controllers that were started
00183         switch_controller([], [get_controller_name(j) for j in self.joints], SwitchControllerRequest.BEST_EFFORT)        
00184 
00185         # kill controllers that were loaded
00186         for j in self.joints:
00187             unload_controller(get_controller_name(j))
00188 
00189     def is_calibrated(self):
00190         # check if joints are calibrated
00191         for j in self.joints:
00192             if self.services[j]().is_calibrated:
00193                 rospy.logdebug("joint %s is already calibrated"%j)
00194             else:
00195                 rospy.logdebug("joint %s needs to be calibrated"%j)
00196                 return False
00197         return True
00198 
00199 
00200     def calibrate(self):
00201         # start all calibration controllers
00202         rospy.logdebug("Start calibrating joints %s"%self.joints)
00203         switch_controller([get_controller_name(j) for j in self.joints], [], SwitchControllerRequest.STRICT)
00204 
00205         # wait for joints to calibrate
00206         self.status.publish(self.joints)
00207         start_time = rospy.Time.now()
00208         while not self.is_calibrated():
00209             if motors_halted and motors_halted == 1:
00210                 diagnostics(2, 'Calibration on hold', 'Calibration is on hold because motors are halted. Enable the run-stop')
00211                 start_time = rospy.Time.now()
00212                 rospy.sleep(1.0)
00213             elif rospy.Time.now() > start_time + rospy.Duration(30.0):  # time for spine to go up is 29 seconds
00214                 diagnostics(2, 'Calibration stuck', 'Joint %s is taking a long time to calibrate. It might be stuck and need some human help'%self.joints)
00215                 rospy.sleep(1.0)                    
00216             rospy.sleep(0.1)
00217 
00218         rospy.logdebug("Stop calibration controllers for joints %s"%self.joints)
00219         switch_controller([], [get_controller_name(j) for j in self.joints], SwitchControllerRequest.BEST_EFFORT)
00220 
00221         # hold joints in place
00222         rospy.logdebug("Loading holding controllers for joints %s"%self.joints)
00223         self.hold_controllers = []
00224         for j in self.joints:
00225             if j in hold_position:
00226                 holder = HoldingController(j)
00227                 holder.hold(hold_position[j])
00228                 self.hold_controllers.append(holder)
00229         self.status.publish()
00230 
00231 
00232 
00233 class CalibrateSequence:
00234     def __init__(self, sequence, status):
00235         self.status = status
00236 
00237         # create CalibrateParallel for all groups in sequence
00238         self.groups = []
00239         for s in sequence:
00240             self.groups.append(CalibrateParallel(s, status))
00241 
00242 
00243     def is_calibrated(self):
00244         # check if all groups in sequence are calibrated
00245         for g in self.groups:
00246             if not g.is_calibrated():
00247                 return False
00248         return True
00249         
00250 
00251     def calibrate(self):
00252         # Check if this sequence needs calibration
00253         if self.is_calibrated():
00254             return True
00255         
00256         # calibrate all groups in sequence
00257         for g in self.groups:
00258             g.calibrate()
00259 
00260 
00261 
00262 
00263 
00264             
00265 def main():
00266     try:
00267         rospy.init_node('calibration', anonymous=True, disable_signals=True)
00268         calibration_start_time = rospy.Time.now()
00269 
00270         rospy.wait_for_service('pr2_controller_manager/load_controller')
00271         rospy.wait_for_service('pr2_controller_manager/switch_controller')
00272         rospy.wait_for_service('pr2_controller_manager/unload_controller')
00273 
00274         
00275         # parse options
00276         allowed_flags = ['alpha-casters', 'alpha-head', 'alpha2b-head', 'arms=', 'force_calibration', 'recalibrate']
00277         opts, args = getopt.gnu_getopt(rospy.myargv(), 'h', allowed_flags)
00278         caster_list = ['caster_fl', 'caster_fr', 'caster_bl', 'caster_br']
00279         head_list = ['head_pan', 'head_tilt']
00280         arms = 'auto'
00281         recalibrate = False
00282         global force_calibration
00283         for o, a in opts:
00284             if o == '-h':
00285                 rospy.loginfo("Flags:", ' '.join(['--'+f for f in allowed_flags]))
00286                 sys.exit(0)
00287             elif o == '--alpha-casters':
00288                 caster_list = ['caster_fl_alpha2', 'caster_fr_alpha2', 'caster_bl_alpha2', 'caster_br_alpha2']
00289             elif o == '--alpha-head':
00290                 head_list = ['head_pan_alpha2', 'head_tilt']
00291             elif o == '--alpha2b-head':
00292                 head_list = ['head_pan_alpha2', 'head_tilt_alpha2b']
00293             elif o == '--arms':
00294                 arms = a
00295             elif o == '--force_calibration':
00296                 force_calibration = True
00297             elif o == '--recalibrate':
00298                 recalibrate = True
00299                 force_calibration = True
00300 
00301         if arms not in ['both', 'none', 'left', 'right', 'auto']:
00302             print 'Arms must be "both", "none", "left", "right", or "auto"'
00303             sys.exit(1)
00304 
00305         # load controller configuration
00306         rospy.loginfo("Loading controller configuration on parameter server...")
00307         pr2_controller_configuration_dir = roslib.packages.get_pkg_dir('pr2_controller_configuration')
00308         calibration_yaml = '%s/pr2_calibration_controllers.yaml' % pr2_controller_configuration_dir
00309         hold_yaml = '%s/pr2_joint_position_controllers.yaml' % pr2_controller_configuration_dir
00310         if len(args) < 3:
00311             rospy.loginfo("No yaml files specified for calibration and holding controllers, using defaults")
00312         else:
00313             calibration_yaml = args[1]
00314             hold_yaml  = args[2]
00315         rospy.set_param(calibration_params_namespace+"/calibrate", yaml.load(open(calibration_yaml)))
00316         rospy.set_param(calibration_params_namespace+"/hold", yaml.load(open(hold_yaml)))
00317 
00318         # status publishing
00319         imustatus = True
00320         joints_status = False
00321         if not recalibrate:
00322             pub_calibrated = rospy.Publisher('calibrated', Bool, latch=True)
00323             pub_calibrated.publish(False)
00324         status = StatusPub()
00325 
00326         # Auto arm selection, determined based on which joints exist.
00327         if arms == 'auto':
00328             started_waiting = rospy.get_rostime()
00329             global last_joint_states
00330             while rospy.get_rostime() <= started_waiting + rospy.Duration(50.0):
00331                 if last_joint_states:
00332                     break
00333             js = last_joint_states
00334             if not js:
00335                 rospy.logwarn('Could not do auto arm selection because no joint state was received')
00336                 arms = 'both'
00337             else:
00338                 if 'r_shoulder_pan_joint' in js.name and 'l_shoulder_pan_joint' in js.name:
00339                     arms = 'both'
00340                 elif 'r_shoulder_pan_joint' in js.name:
00341                     arms = 'right'
00342                 elif 'l_shoulder_pan_joint' in js.name:
00343                     arms = 'left'
00344                 else:
00345                     arms = 'none'
00346             rospy.loginfo("Arm selection was set to \"auto\".  Chose to calibrate using \"%s\"" % arms)
00347 
00348         # define calibration sequence objects
00349         torso = CalibrateSequence([['torso_lift']], status)
00350         gripper_list = []
00351         arm_list = []
00352         if arms == 'both':
00353             arm_list = [['r_shoulder_pan', 'l_shoulder_pan'], ['r_elbow_flex', 'l_elbow_flex'],
00354                         ['r_upper_arm_roll', 'l_upper_arm_roll'], ['r_shoulder_lift', 'l_shoulder_lift'],
00355                         ['r_forearm_roll', 'r_wrist', 'l_forearm_roll', 'l_wrist']]
00356             gripper_list = ['r_gripper', 'l_gripper']
00357         if arms == 'left':
00358             arm_list = [['l_shoulder_pan'], ['l_elbow_flex'], ['l_upper_arm_roll'], ['l_shoulder_lift'], ['l_forearm_roll', 'l_wrist']]
00359             gripper_list = ['l_gripper']
00360         if arms == 'right':
00361             arm_list = [['r_shoulder_pan'], ['r_elbow_flex'], ['r_upper_arm_roll'], ['r_shoulder_lift'], ['r_forearm_roll', 'r_wrist']]
00362             gripper_list = ['r_gripper']
00363         arm = CalibrateSequence(arm_list, status)
00364         gripper = CalibrateSequence([gripper_list], status)
00365         head = CalibrateSequence([head_list, ['laser_tilt']], status)
00366         caster = CalibrateSequence([caster_list], status)
00367 
00368         # if recalibrating, stop all running controllers first
00369         if recalibrate:
00370             controller_list = list_controllers()
00371             def is_running(c) : return c[1]=='running'
00372             running_controllers = [c[0] for c in filter(is_running, zip(controller_list.controllers, controller_list.state))]
00373             print "Running controllers : ", running_controllers
00374             if not switch_controller([], running_controllers, SwitchControllerRequest.STRICT):
00375                 print "Failed to stop controllers"
00376                 sys.exit(1)
00377         
00378         # calibrate imu and torso
00379         if not torso.is_calibrated():
00380             rospy.loginfo('Calibrating imu')
00381             status.publish(['imu'])
00382             imu_calibrate_srv_name = 'torso_lift_imu/calibrate'
00383             rospy.wait_for_service(imu_calibrate_srv_name)  
00384             imu_calibrate_srv = rospy.ServiceProxy(imu_calibrate_srv_name, Empty)
00385             try:
00386                 imu_calibrate_srv()
00387                 status.publish()
00388                 # only calibrate torso when imu calibration succeeds
00389             except:
00390                 imustatus = False
00391             rospy.loginfo('Calibrating imu finished')
00392         else:
00393             rospy.loginfo('Not calibrating imu')
00394 
00395         # calibrate torso
00396         torso.calibrate()
00397 
00398         # calibrate arms
00399         torso_holder = None
00400         if not arm.is_calibrated():
00401             torso_holder = HoldingController('torso_lift')
00402             torso_holder.hold(0.25)
00403             rospy.sleep(5.0)
00404             rospy.loginfo('Moving up spine to allow arms to calibrate')
00405             arm.calibrate()
00406             rospy.loginfo('Moving down spine after arm calibration')
00407             torso_holder.hold(0.01)
00408             rospy.sleep(20.0)
00409 
00410         # calibrate rest of robot
00411         gripper.calibrate()
00412         head.calibrate()
00413         caster.calibrate()
00414 
00415         joints_status = True
00416         status.publish()
00417 
00418         
00419     finally:
00420         rospy.loginfo("Bringing down calibration node")
00421 
00422         rospy.set_param(calibration_params_namespace, "")
00423         del arm
00424         del gripper
00425         del torso
00426         del torso_holder
00427         del caster
00428         del head
00429 
00430         if not imustatus and not joints_status:
00431             rospy.logerr("Both mechanism and IMU calibration failed")
00432         elif not joints_status:
00433             rospy.logerr("IMU calibration complete, but mechanism calibration failed")
00434         elif not imustatus:
00435             rospy.logerr("Mechanism calibration complete, but IMU calibration failed.")
00436         else:
00437             rospy.loginfo('Calibration completed in %f sec' %(rospy.Time.now() - calibration_start_time).to_sec())
00438 
00439         if recalibrate:
00440             if not switch_controller(running_controllers, [], SwitchControllerRequest.STRICT):
00441                 print "Could not start previously running controllers"
00442             
00443         if not recalibrate:
00444             if joints_status:
00445                 pub_calibrated.publish(True)
00446             rospy.spin()
00447             
00448 
00449 if __name__ == '__main__': main()


pr2_bringup
Author(s): Wim Meeussen
autogenerated on Fri Dec 6 2013 20:24:07