home_joints.py 文件源码

def main():
    parser = argparse.ArgumentParser()
    parser.add_argument("-t", "--timeout", type=lambda t:abs(float(t)),
            default=60.0, help="[in seconds] Time to wait for joints to home")
    parser.add_argument("-m", "--mode", type=str.upper, default="AUTO",
            choices=['AUTO', 'MANUAL'], help="Mode to home the robot's joints")
    enable_parser = parser.add_mutually_exclusive_group(required=False)
    enable_parser.add_argument("-e", "--enable", action='store_true', dest='enable',
                       help="Try to enable the robot before homing.")
    enable_parser.add_argument("-n", "--no-enable", action='store_false', dest='enable',
                       help="Avoid trying to enable the robot before homing.")
    enable_parser.set_defaults(enable=True)
    args = parser.parse_args(rospy.myargv()[1:])

    rospy.init_node('home_joints_node')
    if args.enable:
        rs = intera_interface.RobotEnable(CHECK_VERSION)
        rs.enable()
    cmd_mode = HomingCommand.MANUAL if args.mode == 'MANUAL' else HomingCommand.AUTO
    rospy.loginfo("Homing joints in '{0}' mode".format(args.mode.capitalize()))
    home_jnts = HomeJoints()
    state = home_jnts.home_robot(mode=cmd_mode, timeout=args.timeout)
    rospy.loginfo("{0} in homing the robot's joints".format("Succeeded" if state else "Failed"))