play_states.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 ###########################
00003 # play_states.py
00004 # This program play all recorded states of the komodo_arm motors from a file. 
00005 ###########################
00006 import roslib; roslib.load_manifest('ric_robot')
00007 import rospy
00008 from std_msgs.msg import Float64
00009 
00010 def play_states():
00011     rospy.init_node('play_states', anonymous=True)
00012     # open file:
00013     f = open('states_log.txt', 'r')
00014     #read from file
00015     data = [map(float, line.split()) for line in f] 
00016     l=len(data)
00017     pub_base_rotation=rospy.Publisher('/komodo_1/base_rotation_controller/command', Float64)
00018     pub_shoulder = rospy.Publisher('/komodo_1/shoulder_controller/command', Float64)
00019     pub_elbow1 = rospy.Publisher('/komodo_1/elbow1_controller/command', Float64)
00020     pub_elbow2 = rospy.Publisher('/komodo_1/elbow2_controller/command', Float64)
00021     pub_wrist = rospy.Publisher('/komodo_1/wrist_controller/command', Float64)
00022     pub_left_finger = rospy.Publisher('/komodo_1/left_finger_controller/command', Float64)
00023     pub_right_finger = rospy.Publisher('/komodo_1/right_finger_controller/command', Float64)
00024     for i in range(0,l):
00025       if i==5:
00026         rospy.sleep(0.1)
00027       br = Float64(data[i][0])
00028       sh = Float64(data[i][1])
00029       e1 = Float64(data[i][2])
00030       e2 = Float64(data[i][3])
00031       wr = Float64(data[i][4])
00032       lf = Float64(data[i][5])
00033       rf = Float64(data[i][6])
00034       rospy.loginfo('base_rotation_'+str(br))
00035       rospy.loginfo('shoulder_'+str(sh))
00036       rospy.loginfo('elbow1_'+str(e1))
00037       rospy.loginfo('elbow2_'+str(e2))
00038       rospy.loginfo('wrist_'+str(wr))
00039       rospy.loginfo('left_finger_'+str(lf))
00040       rospy.loginfo('right_finger_'+str(rf))
00041       pub_base_rotation.publish(br)
00042       pub_shoulder.publish(sh)
00043       pub_elbow1.publish(e1)
00044       pub_elbow2.publish(e2)
00045       pub_wrist.publish(wr)
00046       pub_left_finger.publish(lf)
00047       pub_right_finger.publish(rf)
00048       rospy.sleep(3.0)
00049     
00050 if __name__ == '__main__':
00051     #rospy.loginfo("Starting play_converter.py")
00052     play_states()
00053 


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