save_states.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 ###########################
00003 # save_states.py
00004 # This program records the states of the komodo_arm motors into a file. 
00005 ###########################
00006 import roslib; roslib.load_manifest('ric_robot')
00007 import rospy
00008 from dynamixel_msgs.msg import JointState as dxl_JointState
00009 from sensor_msgs.msg import JointState
00010 
00011 def br_callback(data):       
00012     global msg
00013     msg[0]=data.current_pos
00014 
00015 def sh_callback(data):       
00016     global msg
00017     msg[1]=data.current_pos
00018 
00019 def e1_callback(data):       
00020     global msg
00021     msg[2]=data.current_pos
00022 
00023 def e2_callback(data):       
00024     global msg
00025     msg[3]=data.current_pos
00026 
00027 def wr_callback(data):       
00028     global msg
00029     msg[4]=data.current_pos
00030 
00031 def lf_callback(data):       
00032     global msg
00033     msg[5]=data.current_pos
00034 
00035 def rf_callback(data):       
00036     global msg
00037     msg[6]=data.current_pos
00038 
00039 def save_states():
00040     rospy.init_node('save_states', anonymous=True)
00041     # open file:
00042     f = open('states_log.txt', 'w')
00043     #write to file
00044     #f.write('base_rotation, shoulder, elbow1, elbow2, wrist, left_finger, right_finger\n')
00045     global msg
00046     msg = [0,0,0,0,0,0,0]
00047     rospy.Subscriber("/komodo_1/base_rotation_controller/state", dxl_JointState, br_callback)
00048     rospy.Subscriber("/komodo_1/shoulder_controller/state", dxl_JointState, sh_callback)
00049     rospy.Subscriber("/komodo_1/elbow1_controller/state", dxl_JointState, e1_callback)
00050     rospy.Subscriber("/komodo_1/elbow2_controller/state", dxl_JointState, e2_callback)
00051     rospy.Subscriber("/komodo_1/left_finger_controller/state", dxl_JointState, lf_callback)
00052     rospy.Subscriber("/komodo_1/right_finger_controller/state", dxl_JointState, rf_callback)
00053     rospy.Subscriber("/komodo_1/wrist_controller/state", dxl_JointState, wr_callback)
00054     i=1
00055     while not rospy.is_shutdown():
00056         br=msg[0]
00057         sh=msg[1]
00058         e1=msg[2]
00059         e2=msg[3]
00060         wr=msg[4]
00061         lf=msg[5]
00062         rf=msg[6]
00063         if i>1:
00064           print 'iteration=', i
00065           p= raw_input("Press ENTER to continue or q to quit")
00066           print "input=", p
00067           if p=='q': 
00068              return
00069           print 'msg=', msg
00070           f.write(str(br)+" "+str(sh)+" "+str(e1)+" "+str(e2)+" "+str(wr)+" "+str(lf)+" "+str(rf))
00071           f.write('\n')
00072           rospy.sleep(0.1)
00073         i=i+1
00074         rospy.sleep(0.1)
00075             #rospy.loginfo(msg)
00076 
00077 if __name__ == '__main__':
00078     rospy.loginfo("Starting save_states.py")
00079     save_states()
00080 


ric_robot
Author(s): RoboTiCan
autogenerated on Thu Aug 27 2015 14:40:59