Go to the documentation of this file.00001
00002 __author__ = 'tom'
00003 import rospy
00004 from std_msgs.msg import Float64
00005
00006
00007 def menu():
00008 return '\nPlease select a single number from the option below to move the komodo arm:\n1) Move to home position.\n2) Say hi.\n3) Rotate wrist.\n4) Prepare to hit position. (not the hit itself just to prepare)\n5) Hit!.\n6) somthing.\n0) To exit this program.'
00009
00010
00011 def main():
00012 rospy.init_node("arm_move")
00013 komodo_arm_publishers = [rospy.Publisher('/komodo_1/wrist_controller/command', Float64, queue_size=1),
00014 rospy.Publisher('/komodo_1/shoulder_controller/command', Float64, queue_size=1),
00015 rospy.Publisher('/komodo_1/right_finger_controller/command', Float64, queue_size=1),
00016 rospy.Publisher('/komodo_1/left_finger_controller/command', Float64, queue_size=1),
00017 rospy.Publisher('/komodo_1/elbow2_controller/command', Float64, queue_size=1),
00018 rospy.Publisher('/komodo_1/elbow1_controller/command', Float64, queue_size=1),
00019 rospy.Publisher('/komodo_1/base_rotation_controller/command', Float64, queue_size=1),
00020 ]
00021 choice = -1
00022 try:
00023 while choice != 0 and not rospy.is_shutdown():
00024 print menu()
00025 try:
00026 choice = int(raw_input('>'))
00027 if choice == 1:
00028 komodo_arm_positions = [Float64()] * len(komodo_arm_publishers)
00029 for i in xrange(len(komodo_arm_publishers)):
00030 komodo_arm_positions[i].data = 0.0
00031 komodo_arm_publishers[i].publish(komodo_arm_positions[i])
00032 elif choice == 2:
00033 komodo_arm_positions = [Float64()] * len(komodo_arm_publishers)
00034 komodo_arm_positions[0].data = 0.0
00035 komodo_arm_publishers[0].publish(komodo_arm_positions[0])
00036 komodo_arm_positions[1].data = 0.0
00037 komodo_arm_publishers[1].publish(komodo_arm_positions[1])
00038 komodo_arm_positions[2].data = 0.0
00039 komodo_arm_publishers[2].publish(komodo_arm_positions[2])
00040 komodo_arm_positions[3].data = 0.0
00041 komodo_arm_publishers[3].publish(komodo_arm_positions[3])
00042 komodo_arm_positions[4].data = 0.0
00043 komodo_arm_publishers[4].publish(komodo_arm_positions[4])
00044 komodo_arm_positions[5].data = 0.0
00045 komodo_arm_publishers[5].publish(komodo_arm_positions[5])
00046 komodo_arm_positions[6].data = 0.0
00047 komodo_arm_publishers[6].publish(komodo_arm_positions[6])
00048 rospy.sleep(3.0)
00049
00050 komodo_arm_positions = [Float64()] * len(komodo_arm_publishers)
00051 komodo_arm_positions[0].data = 0.0
00052 komodo_arm_publishers[0].publish(komodo_arm_positions[0])
00053 komodo_arm_positions[1].data = 0.0
00054 komodo_arm_publishers[1].publish(komodo_arm_positions[1])
00055 komodo_arm_positions[2].data = 0.0
00056 komodo_arm_publishers[2].publish(komodo_arm_positions[2])
00057 komodo_arm_positions[3].data = 0.0
00058 komodo_arm_publishers[3].publish(komodo_arm_positions[3])
00059 komodo_arm_positions[4].data = 0.0
00060 komodo_arm_publishers[4].publish(komodo_arm_positions[4])
00061 komodo_arm_positions[5].data = 0.5
00062 komodo_arm_publishers[5].publish(komodo_arm_positions[5])
00063 komodo_arm_positions[6].data = 0.0
00064 komodo_arm_publishers[6].publish(komodo_arm_positions[6])
00065 rospy.sleep(3.0)
00066
00067 komodo_arm_positions = [Float64()] * len(komodo_arm_publishers)
00068 komodo_arm_positions[0].data = 0.0
00069 komodo_arm_publishers[0].publish(komodo_arm_positions[0])
00070 komodo_arm_positions[1].data = 0.0
00071 komodo_arm_publishers[1].publish(komodo_arm_positions[1])
00072 komodo_arm_positions[2].data = -0.5
00073 komodo_arm_publishers[2].publish(komodo_arm_positions[2])
00074 komodo_arm_positions[3].data = 0.5
00075 komodo_arm_publishers[3].publish(komodo_arm_positions[3])
00076 komodo_arm_positions[4].data = 0.0
00077 komodo_arm_publishers[4].publish(komodo_arm_positions[4])
00078 komodo_arm_positions[5].data = -0.5
00079 komodo_arm_publishers[5].publish(komodo_arm_positions[5])
00080 komodo_arm_positions[6].data = 0.0
00081 komodo_arm_publishers[6].publish(komodo_arm_positions[6])
00082 rospy.sleep(5.0)
00083
00084 komodo_arm_positions = [Float64()] * len(komodo_arm_publishers)
00085 komodo_arm_positions[0].data = 0.0
00086 komodo_arm_publishers[0].publish(komodo_arm_positions[0])
00087 komodo_arm_positions[1].data = 0.0
00088 komodo_arm_publishers[1].publish(komodo_arm_positions[1])
00089 komodo_arm_positions[2].data = 0.0
00090 komodo_arm_publishers[2].publish(komodo_arm_positions[2])
00091 komodo_arm_positions[3].data = 0.0
00092 komodo_arm_publishers[3].publish(komodo_arm_positions[3])
00093 komodo_arm_positions[4].data = 0.0
00094 komodo_arm_publishers[4].publish(komodo_arm_positions[4])
00095 komodo_arm_positions[5].data = 0.0
00096 komodo_arm_publishers[5].publish(komodo_arm_positions[5])
00097 komodo_arm_positions[6].data = 0.0
00098 komodo_arm_publishers[6].publish(komodo_arm_positions[6])
00099 elif choice == 3:
00100 msg = Float64()
00101 msg.data = 1.0
00102 komodo_arm_publishers[0].publish(msg)
00103 rospy.sleep(2.0)
00104 msg = Float64()
00105 msg.data = -1.0
00106 komodo_arm_publishers[0].publish(msg)
00107 rospy.sleep(2.0)
00108
00109 elif choice == 4:
00110 komodo_arm_positions = [Float64()] * len(komodo_arm_publishers)
00111 komodo_arm_positions[0].data = 0.0
00112 komodo_arm_publishers[0].publish(komodo_arm_positions[0])
00113 komodo_arm_positions[1].data = 0.7
00114 komodo_arm_publishers[1].publish(komodo_arm_positions[1])
00115 komodo_arm_positions[2].data = -0.5
00116 komodo_arm_publishers[2].publish(komodo_arm_positions[2])
00117 komodo_arm_positions[3].data = 0.5
00118 komodo_arm_publishers[3].publish(komodo_arm_positions[3])
00119 komodo_arm_positions[4].data = 0.8
00120 komodo_arm_publishers[4].publish(komodo_arm_positions[4])
00121 komodo_arm_positions[5].data = 0.0
00122 komodo_arm_publishers[5].publish(komodo_arm_positions[5])
00123 komodo_arm_positions[6].data = -1.0
00124 komodo_arm_publishers[6].publish(komodo_arm_positions[6])
00125
00126 elif choice == 5:
00127 msg = Float64()
00128 msg.data = 1.0
00129 komodo_arm_publishers[6].publish(msg)
00130 elif choice == 6:
00131 komodo_arm_positions = [Float64()] * len(komodo_arm_publishers)
00132 komodo_arm_positions[0].data = 0.0
00133 komodo_arm_publishers[0].publish(komodo_arm_positions[0])
00134 komodo_arm_positions[1].data = 0.0
00135 komodo_arm_publishers[1].publish(komodo_arm_positions[1])
00136 komodo_arm_positions[2].data = 0.0
00137 komodo_arm_publishers[2].publish(komodo_arm_positions[2])
00138 komodo_arm_positions[3].data = 0.0
00139 komodo_arm_publishers[3].publish(komodo_arm_positions[3])
00140 komodo_arm_positions[4].data = 0.7
00141 komodo_arm_publishers[4].publish(komodo_arm_positions[4])
00142 komodo_arm_positions[5].data = 0.7
00143 komodo_arm_publishers[5].publish(komodo_arm_positions[5])
00144 rospy.sleep(1.0)
00145 komodo_arm_positions[6].data = -1.0
00146 komodo_arm_publishers[6].publish(komodo_arm_positions[6])
00147 elif choice != 0:
00148 rospy.loginfo("Invalid choice, please enter your choice between 0-6.")
00149
00150 except ValueError:
00151 rospy.logerr('Invalid Integer, this program only except integers as choices')
00152 except KeyboardInterrupt:
00153 pass
00154 finally:
00155 rospy.loginfo('Have a nice day')
00156
00157
00158 if __name__ == '__main__':
00159 main()