old_ar10_rviz_control_node.py 文件源码

def main():

    def listener():


        rospy.init_node('listener', anonymous=True) # defines anonymous listener node
        rospy.Subscriber('joint_states',JointState,callback)
        rospy.spin()  # spin() simply keeps python from exiting until this node is stopped

    def callback(msg): # callback is executed when a message is published to 'joint_states'
        pos = [1,1,1,1,1,1,1,1,1,1] # creates an array of 10 to store joint_states
        for i in range (0,10):      # for all servos ...
          pos[i]=msg.position[i+14]  #stores joint states to pos[] while bypassing the initial 14 passive revolute joints
          pos[i]=converter(pos[i])   #converts servo posisitons into commands for the AR10 hand
          print int(pos[i])
        hand.move(0,int(pos[0]))     #sends commands to the AR10
        hand.move(1,int(pos[1]))
        hand.move(2,int(pos[2]))
        hand.move(3,int(pos[3]))
        hand.move(4,int(pos[4]))
            hand.move(5,int(pos[5]))
            hand.move(6,int(pos[6]))
            hand.move(7,int(pos[7]))
            hand.move(8,int(pos[8]))
            hand.move(9,int(pos[9]))
        return pos


    def converter(pos):              #Converter that is executed when called in callback
        command_value = (((pos*-3500)/0.02)+8000)
        return command_value


    hand = ar10() # create hand object
    listener() # start subscriber