tare.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     TareEnable,
00042 )
00043 
00044 
00045 class Tare(baxter_interface.RobustController):
00046     def __init__(self, limb):
00047         """
00048         Wrapper to run the Tare RobustController.
00049 
00050         @param limb: Limb to run tare on [left/right]
00051         """
00052         enable_msg = TareEnable(isEnabled=True, uid='sdk')
00053         enable_msg.data.tuneGravitySpring = True
00054 
00055         disable_msg = TareEnable(isEnabled=False, uid='sdk')
00056 
00057         # Initialize RobustController, use 5 minute timeout for the Tare
00058         # process.
00059         super(Tare, self).__init__(
00060             'robustcontroller/%s/Tare' % (limb,),
00061             enable_msg,
00062             disable_msg,
00063             5 * 60)
00064 
00065 
00066 def gripper_removed(side):
00067     """
00068     Verify grippers are removed for calibration/tare.
00069     """
00070     gripper = baxter_interface.Gripper(side)
00071     if gripper.type() != 'custom':
00072         rospy.logerr("Cannot tare with grippers attached."
00073                        " Remove grippers before tare!")
00074         return False
00075     return True
00076 
00077 
00078 def main():
00079     parser = argparse.ArgumentParser()
00080     required = parser.add_argument_group('required arguments')
00081     required.add_argument('-l', '--limb', required=True,
00082                         choices=['left', 'right'],
00083                         help='Tare the specified limb')
00084     args = parser.parse_args(rospy.myargv()[1:])
00085     limb = args.limb
00086 
00087     print("Initializing node...")
00088     rospy.init_node('rsdk_tare_%s' % (limb,))
00089 
00090     print("Preparing to tare...")
00091     gripper_warn = ("\nIMPORTANT: Make sure to remove grippers and other"
00092                     " attachments before running tare.\n")
00093     print(gripper_warn)
00094     if not gripper_removed(args.limb):
00095         return 1
00096 
00097     rs = baxter_interface.RobotEnable(CHECK_VERSION)
00098     rs.enable()
00099     tt = Tare(limb)
00100     rospy.loginfo("Running tare on %s limb" % (limb,))
00101 
00102     error = None
00103     try:
00104         tt.run()
00105     except Exception, e:
00106         error = e.strerror
00107     finally:
00108         try:
00109             rs.disable()
00110         except Exception:
00111             pass
00112 
00113     if error == None:
00114         rospy.loginfo("Tare finished")
00115     else:
00116         rospy.logerr("Tare failed: %s" % (error,))
00117 
00118     return 0 if error == None else 1
00119 
00120 if __name__ == '__main__':
00121     sys.exit(main())


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