Created
October 3, 2019 15:25
-
-
Save pythongo1/e1fb87d139a066a854a9876df5813c5f to your computer and use it in GitHub Desktop.
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
#!/usr/bin/env python | |
import rospy | |
from std_msgs.msg import Float64 | |
from control_msgs.msg import JointControllerState | |
import math # for sin | |
process_value_data = 0 | |
def callbackstate(subdata_state): | |
#rospy.loginfo("process value is %f", subdata_state.process_value) | |
global process_value_data | |
process_value_data = subdata_state.process_value | |
def movehand(): | |
rospy.init_node("my_hand_command", anonymous=True) | |
finger5_0_publisher = rospy.Publisher("/finger5_0_controller/command", Float64, queue_size=20) | |
finger5_1_publisher = rospy.Publisher("/finger5_1_controller/command", Float64, queue_size=20) | |
finger4_0_publisher = rospy.Publisher("/finger4_0_controller/command", Float64, queue_size=20) | |
finger4_1_publisher = rospy.Publisher("/finger4_1_controller/command", Float64, queue_size=20) | |
finger3_0_publisher = rospy.Publisher("/finger3_0_controller/command", Float64, queue_size=20) | |
finger3_1_publisher = rospy.Publisher("/finger3_1_controller/command", Float64, queue_size=20) | |
finger2_0_publisher = rospy.Publisher("/finger2_0_controller/command", Float64, queue_size=20) | |
finger2_1_publisher = rospy.Publisher("/finger2_1_controller/command", Float64, queue_size=20) | |
finger1_rotate_publisher = rospy.Publisher("/finger1_rotate_axis_controller/command", Float64, queue_size=20) | |
finger1_0_publisher = rospy.Publisher("/finger1_0_base_controller/command", Float64, queue_size=20) | |
finger1_1_publisher = rospy.Publisher("/finger1_1_controller/command", Float64, queue_size=20) | |
move_msg = Float64() | |
rospy.Subscriber('/finger5_0_controller/state', JointControllerState, callbackstate) | |
# back all joint to zero | |
count = 0 | |
rate = rospy.Rate(15) #15 hz | |
while not rospy.is_shutdown(): | |
#t0 = rospy.Time.now().to_sec() | |
angle = 0 | |
#angle = angle - 6.28 # better to look | |
#rospy.loginfo("angle = %f , time = %f", angle, t0) | |
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) | |
#rospy.loginfo("angle = %f ,command_data = %f , process_value_data = %f", angle, command_data, process_value_data) | |
# print do not need global | |
count = count + 1 | |
rospy.loginfo("count = %f ", count) | |
if count == 45: | |
break | |
rate.sleep() | |
rospy.loginfo("count 45 jump out") | |
# move finger5 | |
count = 0 | |
rate_1 = rospy.Rate(15) | |
while not rospy.is_shutdown(): | |
angle = 1.7 | |
move_msg.data = angle | |
finger5_0_publisher.publish(move_msg) | |
angle = 1 | |
move_msg.data = angle | |
finger5_1_publisher.publish(move_msg) | |
count = count + 1 | |
rospy.loginfo("count = %f ", count) | |
if count == 45: | |
break | |
rate_1.sleep() | |
rospy.loginfo("count 45 jump out") | |
# move finger4 | |
count = 0 | |
rate_2 = rospy.Rate(15) | |
while not rospy.is_shutdown(): | |
angle = 1.7 | |
move_msg.data = angle | |
finger4_0_publisher.publish(move_msg) | |
angle = 1 | |
move_msg.data = angle | |
finger4_1_publisher.publish(move_msg) | |
count = count + 1 | |
rospy.loginfo("count = %f ", count) | |
if count == 45: | |
break | |
rate_2.sleep() | |
rospy.loginfo("count 45 jump out") | |
# move finger3 | |
count = 0 | |
rate_3 = rospy.Rate(15) | |
while not rospy.is_shutdown(): | |
angle = 1.7 | |
move_msg.data = angle | |
finger3_0_publisher.publish(move_msg) | |
angle = 1 | |
move_msg.data = angle | |
finger3_1_publisher.publish(move_msg) | |
count = count + 1 | |
rospy.loginfo("count = %f ", count) | |
if count == 45: | |
break | |
rate_3.sleep() | |
rospy.loginfo("count 45 jump out") | |
# move finger2 | |
count = 0 | |
rate_4 = rospy.Rate(15) | |
while not rospy.is_shutdown(): | |
angle = 1.7 | |
move_msg.data = angle | |
finger2_0_publisher.publish(move_msg) | |
angle = 1 | |
move_msg.data = angle | |
finger2_1_publisher.publish(move_msg) | |
count = count + 1 | |
rospy.loginfo("count = %f ", count) | |
if count == 45: | |
break | |
rate_4.sleep() | |
rospy.loginfo("count 45 jump out") | |
# move finger1 | |
count = 0 | |
rate_5 = rospy.Rate(15) | |
while not rospy.is_shutdown(): | |
angle = -1 | |
move_msg.data = angle | |
finger1_rotate_publisher.publish(move_msg) | |
angle = -0.1 | |
move_msg.data = angle | |
finger1_0_publisher.publish(move_msg) | |
angle = 0.5 | |
move_msg.data = angle | |
finger1_1_publisher.publish(move_msg) | |
count = count + 1 | |
rospy.loginfo("count = %f ", count) | |
if count == 45: | |
break | |
rate_5.sleep() | |
rospy.loginfo("count 45 jump out") | |
# back all joint to zero | |
count = 0 | |
rate_6 = rospy.Rate(15) #15 hz | |
while not rospy.is_shutdown(): | |
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) | |
count = count + 1 | |
rospy.loginfo("count = %f ", count) | |
if count == 45: | |
break | |
rate_6.sleep() | |
rospy.loginfo("count 45 jump out") | |
rospy.spin() | |
if __name__ == '__main__': | |
try: | |
movehand() | |
except rospy.ROSInterruptException: pass | |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment