calibrate_arm.py
Go to the documentation of this file.
00001 #!/usr/bin/python2
00002 
00003 # Copyright (c) 2013-2014, Rethink Robotics
00004 # All rights reserved.
00005 #
00006 # Redistribution and use in source and binary forms, with or without
00007 # modification, are permitted provided that the following conditions are met:
00008 #
00009 # 1. Redistributions of source code must retain the above copyright notice,
00010 #    this list of conditions and the following disclaimer.
00011 # 2. Redistributions in binary form must reproduce the above copyright
00012 #    notice, this list of conditions and the following disclaimer in the
00013 #    documentation and/or other materials provided with the distribution.
00014 # 3. Neither the name of the Rethink Robotics nor the names of its
00015 #    contributors may be used to endorse or promote products derived from
00016 #    this software without specific prior written permission.
00017 #
00018 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00019 # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00020 # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00021 # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00022 # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00023 # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00024 # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00025 # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00026 # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00027 # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00028 # POSSIBILITY OF SUCH DAMAGE.
00029 
00030 
00031 import argparse
00032 import os
00033 import sys
00034 
00035 import rospy
00036 
00037 import baxter_interface
00038 
00039 from baxter_interface import CHECK_VERSION
00040 from baxter_maintenance_msgs.msg import (
00041     CalibrateArmEnable,
00042 )
00043 
00044 
00045 class CalibrateArm(baxter_interface.RobustController):
00046     def __init__(self, limb):
00047         """
00048         Wrapper to run the CalibrateArm RobustController.
00049 
00050         @param limb: Limb to run CalibrateArm on [left/right]
00051         """
00052         enable_msg = CalibrateArmEnable(isEnabled=True, uid='sdk')
00053 
00054         disable_msg = CalibrateArmEnable(isEnabled=False, uid='sdk')
00055 
00056         # Initialize RobustController, use 10 minute timeout for the
00057         # CalibrateArm process
00058         super(CalibrateArm, self).__init__(
00059             'robustcontroller/%s/CalibrateArm' % (limb,),
00060             enable_msg,
00061             disable_msg,
00062             10 * 60)
00063 
00064 
00065 def gripper_removed(side):
00066     """
00067     Verify grippers are removed for calibration/tare.
00068     """
00069     gripper = baxter_interface.Gripper(side)
00070     if gripper.type() != 'custom':
00071         rospy.logerr("Cannot calibrate with grippers attached."
00072                        " Remove grippers before calibration!")
00073         return False
00074     return True
00075 
00076 
00077 def main():
00078     parser = argparse.ArgumentParser()
00079     required = parser.add_argument_group('required arguments')
00080     required.add_argument('-l', '--limb', required=True,
00081                         choices=['left', 'right'],
00082                         help="Calibrate the specified arm")
00083     args = parser.parse_args(rospy.myargv()[1:])
00084     arm = args.limb
00085 
00086     print("Initializing node...")
00087     rospy.init_node('rsdk_calibrate_arm_%s' % (arm,))
00088 
00089     print("Preparing to calibrate...")
00090     gripper_warn = ("\nIMPORTANT: Make sure to remove grippers and other"
00091                     " attachments before running calibrate.\n")
00092     print(gripper_warn)
00093     if not gripper_removed(args.limb):
00094         return 1
00095 
00096     rs = baxter_interface.RobotEnable(CHECK_VERSION)
00097     rs.enable()
00098     cat = CalibrateArm(arm)
00099     rospy.loginfo("Running calibrate on %s arm" % (arm,))
00100 
00101     error = None
00102     try:
00103         cat.run()
00104     except Exception, e:
00105         error = e.strerror
00106     finally:
00107         try:
00108             rs.disable()
00109         except Exception:
00110             pass
00111 
00112     if error == None:
00113         rospy.loginfo("Calibrate arm finished")
00114     else:
00115         rospy.logerr("Calibrate arm failed: %s" % (error,))
00116 
00117     return 0 if error == None else 1
00118 
00119 if __name__ == '__main__':
00120     sys.exit(main())


baxter_tools
Author(s): Rethink Robotics Inc.
autogenerated on Sun Oct 5 2014 22:29:27