make_samples.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 # capture samples!!!!1!one
00004 
00005 # this script should eventually be replaced by something that finds
00006 # samples automatically
00007 
00008 import roslib; roslib.load_manifest("maxwell_defs"); 
00009 
00010 import rospy
00011 from sensor_msgs.msg import JointState
00012 
00013 import string
00014 
00015 header1 = """camera_measurements:
00016 - {cam_id: head_camera, config: small_cb_4x5}
00017 joint_commands:
00018 - controller: head_traj_controller
00019   segments:
00020   - duration: 2.0
00021     positions: """
00022 
00023 header2 = """- controller: arm_controller
00024   segments:
00025   - duration: 2.0
00026     positions: """
00027 
00028 header3 = """joint_measurements:
00029 - {chain_id: head_chain, config: tight_tol}
00030 - {chain_id: arm_chain, config: tight_tol}
00031 sample_id: arm_"""
00032 
00033 header4 = """target: {chain_id: arm_chain, target_id: small_cb_4x5}"""
00034 
00035 class SampleMaker:
00036     
00037     def __init__(self):
00038         rospy.init_node("make_samples")
00039         rospy.Subscriber("joint_states", JointState, self.callback)
00040 
00041         self.arm_joints = ["arm_lift_joint", "arm_shoulder_pan_joint", "arm_shoulder_lift_joint", "arm_elbow_flex_joint", "arm_wrist_flex_joint", "arm_wrist_roll_joint"]
00042         self.arm_state = [0.0 for joint in self.arm_joints]
00043         self.head_joints = ["head_pan_joint", "head_tilt_joint"]
00044         self.head_state = [0.0 for joint in self.head_joints]
00045         self.count = 0
00046         
00047         while not rospy.is_shutdown():
00048             print "Move arm/head to a new sample position."
00049             resp = raw_input("press <enter> ")
00050             if string.upper(resp) == "EXIT":
00051                 break
00052             else:
00053                 # save a sample:
00054                 count = str(self.count).zfill(4)
00055                 f = open("samples/arm/arm_"+count+".yaml", "w")
00056                 f.write(header1)
00057                 print>>f, self.head_state
00058                 f.write(header2)
00059                 print>>f, self.arm_state
00060                 f.write(header3)
00061                 print>>f, count
00062                 f.write(header4)
00063             self.count += 1
00064 
00065     def callback(self, msg):
00066         for i in range(len(self.arm_joints)):
00067             try:
00068                 idx = msg.name.index(self.arm_joints[i])
00069                 self.arm_state[i] = msg.position[idx]
00070             except: 
00071                 pass
00072         for i in range(len(self.head_joints)):
00073             try:
00074                 idx = msg.name.index(self.head_joints[i])
00075                 self.head_state[i] = msg.position[idx]
00076             except: 
00077                 pass
00078 
00079 
00080 if __name__=="__main__":
00081     SampleMaker()
00082 


maxwell_calibration
Author(s): Michael Ferguson
autogenerated on Wed Aug 26 2015 12:32:37