port_samples.py
Go to the documentation of this file.
00001 #! /usr/bin/env python
00002 
00003 # This script is used to convert PR2 Samples into PR2 Lite Samples
00004 # Author Vijay Pradeep
00005 # Modified 11/11 by Adam Leeper
00006 
00007 import roslib; roslib.load_manifest('pr2_calibration_launch')
00008 import rospy
00009 import yaml
00010 import sys
00011 import os
00012 
00013 input_dir = rospy.myargv()[1]
00014 output_dir = rospy.myargv()[2]
00015 sample_names = [x for x in os.listdir(input_dir) if ".yaml" in x]
00016 sample_names.sort()
00017 full_input_paths  = [input_dir  + "/" + x for x in sample_names]
00018 full_output_paths = [output_dir + "/" + x for x in sample_names]
00019 
00020 #TODO Add support for left OR right arm
00021 
00022 for cur_input_path, cur_output_path in zip(full_input_paths, full_output_paths):
00023     print "On sample [%s]" % cur_input_path
00024     cur_config = yaml.load(open(cur_input_path))
00025 
00026     # Don't use any of the l_forearm samples
00027     if 'forearm_left_rect' not in [ x['cam_id'] for x in cur_config['camera_measurements']]:
00028         next_config = {}
00029         if 'forearm_right_rect' in [ x['cam_id'] for x in cur_config['camera_measurements']]:
00030             next_config['camera_measurements'] = [{'cam_id':'forearm_right_rect', 'config':'small_cb_4x5'}]
00031         else:
00032             next_config['camera_measurements'] = [{'cam_id':'kinect_rect', 'config':'small_cb_4x5'}, {'cam_id':'prosilica_rect', 'config':'small_cb_4x5'} ]
00033         next_config['joint_commands'] = [x for x in cur_config['joint_commands'] if x['controller'] in ['head_traj_controller', 'r_arm_controller']]
00034         next_config['joint_measurements'] = [x for x in cur_config['joint_measurements'] if x['chain_id'] in ['head_chain', 'right_arm_chain']]
00035         if 'tilt_laser' in [ x['laser_id'] for x in cur_config['laser_measurements']]:
00036             next_config['laser_measurements'] = [{'config': 'small_cb_4x5', 'laser_id': 'tilt_laser'}]
00037         else:
00038             next_config['laser_measurements'] = []
00039         next_config['sample_id'] = cur_config['sample_id']
00040         next_config['target'] = cur_config['target']
00041 
00042         # Tilt the head down by about 11 degrees to account for higher kinect position.
00043         # Yes, this is a total hack.
00044         for head_cmd in [x for x in next_config['joint_commands'] if x['controller'] == 'head_traj_controller']:
00045             for cur_segment in head_cmd['segments']:
00046                 cur_segment['positions'][1] = cur_segment['positions'][1] + 0.2
00047 
00048         outfile = file(cur_output_path, 'w')
00049         yaml.dump(next_config, outfile)
00050         outfile.close()
00051     else:
00052         print "Skipping"
00053 
00054 


pr2_se_calibration_launch
Author(s): Adam Leeper, Vijay Pradeep
autogenerated on Fri Aug 28 2015 12:06:45