SHARE
TWEET

Untitled

a guest Oct 21st, 2019 91 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. #!/usr/bin/env python
  2. import rospy
  3. from std_msgs.msg import Float64
  4. from control_msgs.msg import JointControllerState
  5. import math  # for sin
  6.  
  7. process_value_data = 0
  8.  
  9. def callbackstate(subdata_state):
  10.     #rospy.loginfo("process value is %f", subdata_state.process_value)
  11.     global process_value_data
  12.     process_value_data = subdata_state.process_value
  13.    
  14.  
  15. def movehand():
  16.     rospy.init_node("my_hand_command", anonymous=True)
  17.     finger5_0_publisher = rospy.Publisher("/finger5_0_controller/command", Float64, queue_size=20)
  18.     finger5_1_publisher = rospy.Publisher("/finger5_1_controller/command", Float64, queue_size=20)
  19.     finger4_0_publisher = rospy.Publisher("/finger4_0_controller/command", Float64, queue_size=20)
  20.     finger4_1_publisher = rospy.Publisher("/finger4_1_controller/command", Float64, queue_size=20)
  21.     finger3_0_publisher = rospy.Publisher("/finger3_0_controller/command", Float64, queue_size=20)
  22.     finger3_1_publisher = rospy.Publisher("/finger3_1_controller/command", Float64, queue_size=20)
  23.     finger2_0_publisher = rospy.Publisher("/finger2_0_controller/command", Float64, queue_size=20)
  24.     finger2_1_publisher = rospy.Publisher("/finger2_1_controller/command", Float64, queue_size=20)
  25.     finger1_rotate_publisher = rospy.Publisher("/finger1_rotate_axis_controller/command", Float64, queue_size=20)
  26.     finger1_0_publisher = rospy.Publisher("/finger1_0_base_controller/command", Float64, queue_size=20)
  27.     finger1_1_publisher = rospy.Publisher("/finger1_1_controller/command", Float64, queue_size=20)
  28.     move_msg = Float64()
  29.  
  30.     rospy.Subscriber('/finger5_0_controller/state', JointControllerState, callbackstate)
  31.  
  32.     # back all joint to zero
  33.     count = 0
  34.     rate = rospy.Rate(15)  #15 hz
  35.     while not rospy.is_shutdown():
  36.         #t0 = rospy.Time.now().to_sec()
  37.         angle = 0
  38.         #angle = angle - 6.28    # better to look
  39.         #rospy.loginfo("angle = %f , time = %f", angle, t0)
  40.         move_msg.data = angle
  41.         finger5_0_publisher.publish(move_msg)
  42.         finger5_1_publisher.publish(move_msg)
  43.         finger4_0_publisher.publish(move_msg)
  44.         finger4_1_publisher.publish(move_msg)
  45.         finger3_0_publisher.publish(move_msg)
  46.         finger3_1_publisher.publish(move_msg)
  47.         finger2_0_publisher.publish(move_msg)
  48.         finger2_1_publisher.publish(move_msg)
  49.         finger1_rotate_publisher.publish(move_msg)
  50.         finger1_0_publisher.publish(move_msg)
  51.         finger1_1_publisher.publish(move_msg)
  52.  
  53.         #rospy.loginfo("angle = %f ,command_data = %f , process_value_data = %f", angle, command_data,  process_value_data)
  54.         # print do not need global
  55.         count = count + 1
  56.         rospy.loginfo("count = %f ", count)
  57.         if count == 45:
  58.             break
  59.         rate.sleep()
  60.     rospy.loginfo("count 45 jump out")
  61.  
  62.     # move finger5
  63.     count = 0
  64.     rate_1 = rospy.Rate(15)
  65.     while not rospy.is_shutdown():
  66.         angle = 1.7
  67.         move_msg.data = angle
  68.         finger5_0_publisher.publish(move_msg)
  69.         angle = 1
  70.         move_msg.data = angle
  71.         finger5_1_publisher.publish(move_msg)
  72.  
  73.         count = count + 1
  74.         rospy.loginfo("count = %f ", count)
  75.         if count == 45:
  76.             break
  77.         rate_1.sleep()
  78.     rospy.loginfo("count 45 jump out")
  79.  
  80.     # move finger4
  81.     count = 0
  82.     rate_2 = rospy.Rate(15)
  83.     while not rospy.is_shutdown():
  84.         angle = 1.7
  85.         move_msg.data = angle
  86.         finger4_0_publisher.publish(move_msg)
  87.         angle = 1
  88.         move_msg.data = angle
  89.         finger4_1_publisher.publish(move_msg)
  90.  
  91.         count = count + 1
  92.         rospy.loginfo("count = %f ", count)
  93.         if count == 45:
  94.             break
  95.         rate_2.sleep()
  96.     rospy.loginfo("count 45 jump out")
  97.  
  98.     # move finger3
  99.     count = 0
  100.     rate_3 = rospy.Rate(15)
  101.     while not rospy.is_shutdown():
  102.         angle = 1.7
  103.         move_msg.data = angle
  104.         finger3_0_publisher.publish(move_msg)
  105.         angle = 1
  106.         move_msg.data = angle
  107.         finger3_1_publisher.publish(move_msg)
  108.  
  109.         count = count + 1
  110.         rospy.loginfo("count = %f ", count)
  111.         if count == 45:
  112.             break
  113.         rate_3.sleep()
  114.     rospy.loginfo("count 45 jump out")
  115.  
  116.     # move finger2
  117.     count = 0
  118.     rate_4 = rospy.Rate(15)
  119.     while not rospy.is_shutdown():
  120.         angle = 1.7
  121.         move_msg.data = angle
  122.         finger2_0_publisher.publish(move_msg)
  123.         angle = 1
  124.         move_msg.data = angle
  125.         finger2_1_publisher.publish(move_msg)
  126.  
  127.         count = count + 1
  128.         rospy.loginfo("count = %f ", count)
  129.         if count == 45:
  130.             break
  131.         rate_4.sleep()
  132.     rospy.loginfo("count 45 jump out")
  133.  
  134.     # move finger1
  135.     count = 0
  136.     rate_5 = rospy.Rate(15)
  137.     while not rospy.is_shutdown():
  138.         angle = -1
  139.         move_msg.data = angle
  140.         finger1_rotate_publisher.publish(move_msg)
  141.         angle = -0.1
  142.         move_msg.data = angle
  143.         finger1_0_publisher.publish(move_msg)
  144.         angle = 0.5
  145.         move_msg.data = angle
  146.         finger1_1_publisher.publish(move_msg)
  147.  
  148.         count = count + 1
  149.         rospy.loginfo("count = %f ", count)
  150.         if count == 45:
  151.             break
  152.         rate_5.sleep()
  153.     rospy.loginfo("count 45 jump out")
  154.  
  155.     # back all joint to zero
  156.     count = 0
  157.     rate_6 = rospy.Rate(15)  #15 hz
  158.     while not rospy.is_shutdown():
  159.         angle = 0
  160.         move_msg.data = angle
  161.         finger5_0_publisher.publish(move_msg)
  162.         finger5_1_publisher.publish(move_msg)
  163.         finger4_0_publisher.publish(move_msg)
  164.         finger4_1_publisher.publish(move_msg)
  165.         finger3_0_publisher.publish(move_msg)
  166.         finger3_1_publisher.publish(move_msg)
  167.         finger2_0_publisher.publish(move_msg)
  168.         finger2_1_publisher.publish(move_msg)
  169.         finger1_rotate_publisher.publish(move_msg)
  170.         finger1_0_publisher.publish(move_msg)
  171.         finger1_1_publisher.publish(move_msg)
  172.  
  173.         count = count + 1
  174.         rospy.loginfo("count = %f ", count)
  175.         if count == 45:
  176.             break
  177.         rate_6.sleep()
  178.     rospy.loginfo("count 45 jump out")
  179.  
  180.  
  181.     rospy.spin()
  182.  
  183. if __name__ == '__main__':
  184.     try:
  185.         movehand()
  186.     except rospy.ROSInterruptException: pass
RAW Paste Data
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy. OK, I Understand
 
Top