run_gripper_driver.py 文件源码

def run_gripper_driver():
    # init moveit commander
    moveit_commander.roscpp_initialize(sys.argv)
    # specify move group
    arm_group = moveit_commander.MoveGroupCommander("arm")
    gripper_group = moveit_commander.MoveGroupCommander("gripper")
    # init publisher
    arm_pub = rospy.Publisher('servo_write', JointStatus, queue_size=1)
    gripper_pub = rospy.Publisher('gripper_write', Float32, queue_size=1)
    # init ros node
    rospy.init_node('real_servo_driver', anonymous=True)

    print "============ Waiting for RVIZ..."
    rospy.sleep(2)
    # move grasper to init position
    init_position(arm_group, arm_pub)
    init_gripper(gripper_group, gripper_pub)

    # set ros publisher rate, 10hz = 10 seconds for a circle
    rate = rospy.Rate(100)

    # set position
    close_gripper = {"finger_joint1": 0, "finger_joint2": 0}
    open_gripper = {"finger_joint1": 0.015, "finger_joint2": -0.015}
    switch = True
    while True:
        if switch:
            plan_msg = gripper_group.plan(joints=open_gripper)
        else:
            plan_msg = gripper_group.plan(joints=close_gripper)
        switch = not switch
        gripper_group.execute(plan_msg=plan_msg, wait=False)
        # servo talker
        gripper_talker(gripper_pub, plan_msg, rate)
        rospy.sleep(5)
        if is_exit:
            break
    # shutdown moveit commander
    moveit_commander.roscpp_shutdown()