my_hand_check_version3_function_code.py

#!/usr/bin/env python
import rospy
from std_msgs.msg import Float64
from control_msgs.msg import JointControllerState
import math  # for sin
import os 

# for publish
finger5_0_publisher = 0
finger5_1_publisher = 0
finger4_0_publisher = 0
finger4_1_publisher = 0
finger3_0_publisher = 0
finger3_1_publisher = 0
finger2_0_publisher = 0
finger2_1_publisher = 0
finger1_rotate_publisher = 0
finger1_0_publisher = 0
finger1_1_publisher = 0

# for global variable
finger5_0_process_value = 0
finger5_1_process_value = 0
finger4_0_process_value = 0
finger4_1_process_value = 0
finger3_0_process_value = 0
finger3_1_process_value = 0
finger2_0_process_value = 0
finger2_1_process_value = 0
finger1_rotate_axis_process_value = 0
finger1_0_base_process_value = 0
finger1_1_process_value = 0

def finger5_0_callback(subdata):
    #rospy.loginfo("process value1 is %f", subdata.process_value)
    global finger5_0_process_value
    finger5_0_process_value = subdata.process_value

def finger5_1_callback(subdata):
    #rospy.loginfo("process value2 is %f", subdata.process_value)
    global finger5_1_process_value
    finger5_1_process_value = subdata.process_value

def finger4_0_callback(subdata):
    #rospy.loginfo("process value1 is %f", subdata.process_value)
    global finger4_0_process_value
    finger4_0_process_value = subdata.process_value

def finger4_1_callback(subdata):
    #rospy.loginfo("process value2 is %f", subdata.process_value)
    global finger4_1_process_value
    finger4_1_process_value = subdata.process_value

def finger3_0_callback(subdata):
    #rospy.loginfo("process value1 is %f", subdata.process_value)
    global finger3_0_process_value
    finger3_0_process_value = subdata.process_value

def finger3_1_callback(subdata):
    #rospy.loginfo("process value2 is %f", subdata.process_value)
    global finger3_1_process_value
    finger3_1_process_value = subdata.process_value

def finger2_0_callback(subdata):
    #rospy.loginfo("process value1 is %f", subdata.process_value)
    global finger2_0_process_value
    finger2_0_process_value = subdata.process_value

def finger2_1_callback(subdata):
    #rospy.loginfo("process value2 is %f", subdata.process_value)
    global finger2_1_process_value
    finger2_1_process_value = subdata.process_value

def finger1_rotate_axis_callback(subdata):
    #rospy.loginfo("process value2 is %f", subdata.process_value)
    global finger1_rotate_axis_process_value
    finger1_rotate_axis_process_value = subdata.process_value

def finger1_0_base_callback(subdata):
    #rospy.loginfo("process value1 is %f", subdata.process_value)
    global finger1_0_base_process_value
    finger1_0_base_process_value = subdata.process_value

def finger1_1_callback(subdata):
    #rospy.loginfo("process value2 is %f", subdata.process_value)
    global finger1_1_process_value
    finger1_1_process_value = subdata.process_value


def open_hand():
    # back every joint angle to zero

    move_msg = Float64()

    rate_0 = rospy.Rate(15)  #15 hz
    while not rospy.is_shutdown():
        #rospy.loginfo("angle = %f ,command_data = %f , process_value_data = %f", angle, command_data,  process_value_data)
        # print do not need global
        angle = 0
        move_msg.data = angle

        finger5_0_publisher.publish(move_msg)
        finger5_1_publisher.publish(move_msg)
        finger4_0_publisher.publish(move_msg)
        finger4_1_publisher.publish(move_msg)
        finger3_0_publisher.publish(move_msg)
        finger3_1_publisher.publish(move_msg)
        finger2_0_publisher.publish(move_msg)
        finger2_1_publisher.publish(move_msg)
        finger1_rotate_publisher.publish(move_msg)
        finger1_0_publisher.publish(move_msg)
        finger1_1_publisher.publish(move_msg)

        if (check_is_arrive(finger5_0_process_value, angle, 0.01) and check_is_arrive(finger5_1_process_value, angle, 0.1) and
            check_is_arrive(finger4_0_process_value, angle, 0.01) and check_is_arrive(finger4_1_process_value, angle, 0.1) and
            check_is_arrive(finger3_0_process_value, angle, 0.01) and check_is_arrive(finger3_1_process_value, angle, 0.1) and
            check_is_arrive(finger2_0_process_value, angle, 0.01) and check_is_arrive(finger2_1_process_value, angle, 0.1) and
            check_is_arrive(finger1_rotate_axis_process_value, angle, 0.01) and
            check_is_arrive(finger1_0_base_process_value, angle, 0.01) and check_is_arrive(finger1_1_process_value, angle, 0.1)):
            break


        rate_0.sleep()
    rospy.loginfo("finish open the hand !!!!!!")

def finger_pub_function(finger_number, angle):
    # use finger_number to decide which finger should move
    move_msg = Float64()
    yes_count1 = 0
    yes_count2 = 0
    is_break1 = False
    is_break2 = False

    # finger_number==5 is the finger5, finger_number==4 is the finger4 ... 
    # (special) finger_number==0 is the rotate axis(under the finger1)
    if finger_number == 0:
        rate_1 = rospy.Rate(15)
        while not rospy.is_shutdown():
            move_msg.data = angle

            if not check_is_arrive(finger1_rotate_axis_process_value, angle, 0.01):
                finger1_rotate_publisher.publish(move_msg)
            else:
                 break

            rate_1.sleep()

        # force to end this finger_pub_function
        return 0
        
    if finger_number == 5:
        finger_0_pub = finger5_0_publisher
        finger_1_pub = finger5_1_publisher

    if finger_number == 4:
        finger_0_pub = finger4_0_publisher
        finger_1_pub = finger4_1_publisher

    if finger_number == 3:
        finger_0_pub = finger3_0_publisher
        finger_1_pub = finger3_1_publisher

    if finger_number == 2:
        finger_0_pub = finger2_0_publisher
        finger_1_pub = finger2_1_publisher

    if finger_number == 1:
        finger_0_pub = finger1_0_publisher
        finger_1_pub = finger1_1_publisher


    rate_1 = rospy.Rate(15)
    while not rospy.is_shutdown():
        angle_0 = angle
        move_msg.data = angle_0

        # choose the process value, by finger_number
        # finger_number==5 is the finger5, finger_number==4 is the finger4 ......
        if finger_number == 5:
            process_value_0 = finger5_0_process_value
            process_value_1 = finger5_1_process_value
        if finger_number == 4:
            process_value_0 = finger4_0_process_value
            process_value_1 = finger4_1_process_value
        if finger_number == 3:
            process_value_0 = finger3_0_process_value
            process_value_1 = finger3_1_process_value
        if finger_number == 2:
            process_value_0 = finger2_0_process_value
            process_value_1 = finger2_1_process_value
        if finger_number == 1:
            process_value_0 = finger1_0_base_process_value
            process_value_1 = finger1_1_process_value

        # check the angle  arrive. if not arrive, publish command.
        if not check_is_arrive(process_value_0, angle_0, 0.01):
            finger_0_pub.publish(move_msg)
        else:
            yes_count1 = yes_count1 + 1
            rospy.loginfo("yes_count1 = %f", yes_count1)

        if yes_count1 == 5:
            # break
            is_break1 = True

        # remember to use 2.00 , not 2 !!!
        # because of python, 5/2=2, 5/2.00=2.5
        angle_1 = angle / 2.00

        expect_error = 0.1
        # special angle for finger1, and special epect_error for finger1
        if finger_number == 1:
            angle_1 = angle * 5
            expect_error = 0.01

        move_msg.data = angle_1

        if not check_is_arrive(process_value_1, angle_1, expect_error):
            finger_1_pub.publish(move_msg)
        else:
            yes_count2 = yes_count2 + 1
            rospy.loginfo("yes_count2 = %f", yes_count2)

        if yes_count2 == 15:
            # break
            is_break2 = True

        if is_break1 and is_break2:
            yes_count1 = 0
            yes_count2 = 0
            is_break1 = False
            is_break2 = False
            break

        rate_1.sleep()



def finger5_move(angle):
    # finger5 function , need to add limit
    # if angle is not in this range, exit this program
    if angle < 0 or angle > 1.7:
        rospy.loginfo(" [error! error!][finger5] the angle is not between 0 and 1.7 rad ! ")
        os._exit(0)

    finger_number = 5
    finger_pub_function(finger_number, angle)
    rospy.loginfo("finger5 finish !!!!!!!!!!")



def finger4_move(angle):
    # finger4 function , need to add limit
    # if angle is not in this range, exit this program
    if angle < 0 or angle > 1.7:
        rospy.loginfo(" [error! error!][finger4] the angle is not between 0 and 1.7 rad ! ")
        os._exit(0)

    finger_number = 4
    finger_pub_function(finger_number, angle)
    rospy.loginfo("finger4 finish !!!!!!!!!!")

def finger3_move(angle):
    # finger3 function , need to add limit
    # if angle is not in this range, exit this program
    if angle < 0 or angle > 1.7:
        rospy.loginfo(" [error! error!][finger3] the angle is not between 0 and 1.7 rad ! ")
        os._exit(0)

    finger_number = 3
    finger_pub_function(finger_number, angle)
    rospy.loginfo("finger3 finish !!!!!!!!!!")

def finger2_move(angle):
    # finger2 function , need to add limit
    # if angle is not in this range, exit this program
    if angle < 0 or angle > 1.7:
        rospy.loginfo(" [error! error!][finger2] the angle is not between 0 and 1.7 rad ! ")
        os._exit(0)

    finger_number = 2
    finger_pub_function(finger_number, angle)
    rospy.loginfo("finger2 finish !!!!!!!!!!")

def finger1_move(angle):
    # not include rotate axis
    # finger1 function , need to add limit
    # if angle is not in this range, exit this program
    if angle < 0 or angle > 1.7:
        rospy.loginfo(" [error! error!][finger1] the angle is not between 0 and 1.7 rad ! ")
        os._exit(0)

    finger_number = 1
    finger_pub_function(finger_number, angle)
    rospy.loginfo("finger1 finish !!!!!!!!!!")


def finger1_rotate_move(angle):
    if angle < 0 or angle > 1.7:
        rospy.loginfo(" [error! error!][finger1_ rotate_axis] the angle is not between 0 and 1.7 rad ! ")
        os._exit(0)

    finger_number = 0
    finger_pub_function(finger_number, angle)
    rospy.loginfo("finger1 rotate axis finish !!!!!!!!!!")


def check_is_arrive(finger_arg, expect_value, expect_limit):
    # check finger's position
    difference = finger_arg - expect_value
    origin_difference = difference

    # notice pi's number!!!!!!!!!!!
    while difference < 0 or difference >= 6.28318530718:
        if difference < 0:
            difference = difference + 6.28318530718
        if difference >= 6.28318530718:
            difference = difference - 6.28318530718

    bound_difference = difference
    final_difference = bound_difference
    if difference > 3.14159265359:
        final_difference = 6.28318530718 - difference

    rospy.loginfo("process value is %f, expect is %f, - = %f, 0 ~ 6.28 = %f, final = %f", finger_arg, expect_value, origin_difference, bound_difference, final_difference)

    if final_difference < expect_limit:
        return True
    else:
        return False

def movehand():
    rospy.init_node("my_hand_command", anonymous=True)

    global finger5_0_publisher
    finger5_0_publisher = rospy.Publisher("/finger5_0_controller/command", Float64, queue_size=20)
    global finger5_1_publisher
    finger5_1_publisher = rospy.Publisher("/finger5_1_controller/command", Float64, queue_size=20)
    global finger4_0_publisher
    finger4_0_publisher = rospy.Publisher("/finger4_0_controller/command", Float64, queue_size=20)
    global finger4_1_publisher
    finger4_1_publisher = rospy.Publisher("/finger4_1_controller/command", Float64, queue_size=20)
    global finger3_0_publisher
    finger3_0_publisher = rospy.Publisher("/finger3_0_controller/command", Float64, queue_size=20)
    global finger3_1_publisher
    finger3_1_publisher = rospy.Publisher("/finger3_1_controller/command", Float64, queue_size=20)
    global finger2_0_publisher
    finger2_0_publisher = rospy.Publisher("/finger2_0_controller/command", Float64, queue_size=20)
    global finger2_1_publisher
    finger2_1_publisher = rospy.Publisher("/finger2_1_controller/command", Float64, queue_size=20)
    global finger1_rotate_publisher
    finger1_rotate_publisher = rospy.Publisher("/finger1_rotate_axis_controller/command", Float64, queue_size=20)
    global finger1_0_publisher
    finger1_0_publisher = rospy.Publisher("/finger1_0_base_controller/command", Float64, queue_size=20)
    global finger1_1_publisher
    finger1_1_publisher = rospy.Publisher("/finger1_1_controller/command", Float64, queue_size=20)
    move_msg = Float64()

    rospy.Subscriber('/finger5_0_controller/state', JointControllerState, finger5_0_callback)
    rospy.Subscriber('/finger5_1_controller/state', JointControllerState, finger5_1_callback)
    rospy.Subscriber('/finger4_0_controller/state', JointControllerState, finger4_0_callback)
    rospy.Subscriber('/finger4_1_controller/state', JointControllerState, finger4_1_callback)
    rospy.Subscriber('/finger3_0_controller/state', JointControllerState, finger3_0_callback)
    rospy.Subscriber('/finger3_1_controller/state', JointControllerState, finger3_1_callback)
    rospy.Subscriber('/finger2_0_controller/state', JointControllerState, finger2_0_callback)
    rospy.Subscriber('/finger2_1_controller/state', JointControllerState, finger2_1_callback)

    rospy.Subscriber('/finger1_rotate_axis_controller/state', JointControllerState, finger1_rotate_axis_callback)    
    rospy.Subscriber('/finger1_0_base_controller/state', JointControllerState, finger1_0_base_callback)
    rospy.Subscriber('/finger1_1_controller/state', JointControllerState, finger1_1_callback)


#------------ open hand ---------------------------------------
    open_hand()

#------------ move finger5 -----------------------------------
    angle = 1.7
    finger5_move(angle)

#------------ move finger4  ---------------------------------
    angle = 1.7
    finger4_move(angle)

#------------ move finger3 ----------------------------------
    angle = 1.7
    finger3_move(angle)

#------------ move finger2 ----------------------------------
    angle = 1.7    
    finger2_move(angle)


#------------ move finger1 (not include rotate axis )--------
    angle = 0.1    # 0.1 0.5
    finger1_move(angle)

#------------ move rotate axis(under finger1) --------------
    angle = 1   # 1
    finger1_rotate_move(angle)

#------------- open hand ----------------------------------
    open_hand()


    rospy.spin()

if __name__ == '__main__':
    try:
        movehand()
    except rospy.ROSInterruptException: pass