calibrate_arm.py 文件源码

def main():
    parser = argparse.ArgumentParser()
    parser.add_argument('-l', '--limb',
                        choices=['left', 'right'], default="right",
                        help="Calibrate the specified arm")
    args = parser.parse_args(rospy.myargv()[1:])
    arm = args.limb

    print("Initializing node...")
    rospy.init_node('sdk_calibrate_arm_{0}'.format(arm), disable_signals=True)

    rospy.loginfo("Preparing to Calibrate...")
    gripper_warn = ("IMPORTANT: Make sure to remove grippers and other"
                    " attachments before running Calibrate.")
    rospy.loginfo(gripper_warn)
    if not gripper_removed(args.limb):
        return 1

    ca = CalibrateArm(arm)

    error = None
    goal_state = "unreported error"
    rospy.loginfo("Running Calibrate on {0} arm".format(arm))
    try:
        goal_state = ca.start_calibration()
    except KeyboardInterrupt, e:
        error = e
        goal_state = ca.stop_calibration()

    if error == None and "success" in str(goal_state).lower():
        rospy.loginfo("Calibrate arm finished successfully. Please reboot your robot to use this calibration data.")
    else:
        error = True
        rospy.logerr("Calibrate arm failed with {0}".format(goal_state))
        rospy.logerr("Please re-run this Calibration request.")

    return 0 if error == None else 1