$search
00001 #!/usr/bin/env python 00002 # Software License Agreement (BSD License) 00003 # 00004 # Copyright (c) 2008, Willow Garage, Inc. 00005 # All rights reserved. 00006 # 00007 # Redistribution and use in source and binary forms, with or without 00008 # modification, are permitted provided that the following conditions 00009 # are met: 00010 # 00011 # * Redistributions of source code must retain the above copyright 00012 # notice, this list of conditions and the following disclaimer. 00013 # * Redistributions in binary form must reproduce the above 00014 # copyright notice, this list of conditions and the following 00015 # disclaimer in the documentation and/or other materials provided 00016 # with the distribution. 00017 # * Neither the name of Willow Garage, Inc. nor the names of its 00018 # contributors may be used to endorse or promote products derived 00019 # from this software without specific prior written permission. 00020 # 00021 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00022 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00023 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00024 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00025 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00026 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00027 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00028 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00029 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00030 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00031 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00032 # POSSIBILITY OF SUCH DAMAGE. 00033 00034 # Modified from original executive on 11/11 by Adam Leeper 00035 00036 import roslib; roslib.load_manifest('pr2_calibration_launch') 00037 import rospy 00038 import yaml 00039 import sys 00040 import pr2_calibration_executive.capture_exec 00041 import time 00042 from calibration_msgs.msg import RobotMeasurement 00043 import os 00044 import string 00045 00046 print "Starting executive..." 00047 time.sleep(7.0) 00048 00049 rospy.init_node("pr2_capture_executive_node") 00050 00051 samples_dir = rospy.myargv()[1] 00052 config_dir = rospy.myargv()[2] 00053 00054 executive = pr2_calibration_executive.capture_exec.CaptureExecutive(config_dir) 00055 time.sleep(1.0) 00056 00057 left_sample_names = [x for x in os.listdir(samples_dir + "/left/") if ".yaml" in x] 00058 right_sample_names = [] 00059 far_sample_names = [] 00060 right_sample_names = [x for x in os.listdir(samples_dir + "/right/") if ".yaml" in x] 00061 far_sample_names = [x for x in os.listdir(samples_dir + "/far/") if ".yaml" in x] 00062 00063 left_sample_names.sort() 00064 right_sample_names.sort() 00065 far_sample_names.sort() 00066 00067 print "Left Samples: \n - %s" % "\n - ".join(left_sample_names) 00068 print "Right Samples: \n - %s" % "\n - ".join(right_sample_names) 00069 print "Far Samples: \n - %s" % "\n - ".join(far_sample_names) 00070 00071 pub = rospy.Publisher("robot_measurement", RobotMeasurement) 00072 00073 far_success_count = 0 00074 left_success_count = 0 00075 left_fail_count = 0 00076 right_success_count = 0 00077 right_fail_count = 0 00078 00079 # TODO Add support for right OR left arm 00080 capture_right_arm_data = True 00081 capture_left_arm_data = False 00082 00083 try: 00084 # Capture Far Checkerboards 00085 keep_collecting = True 00086 full_paths = [samples_dir + "/far/" + x for x in far_sample_names] 00087 cur_config = yaml.load(open(full_paths[0])) 00088 m_robot = executive.capture(cur_config, rospy.Duration(0.01)) 00089 00090 while not rospy.is_shutdown() and keep_collecting: 00091 00092 print "Please place the large 7x6 checkerboard approx 3m in front of the robot" 00093 print "in view of the head cameras and tilting laser." 00094 print "Press <enter> when ready to collect data, or type \"N\" if done collecting large checkerboards" 00095 resp = raw_input("> ") 00096 if string.upper(resp) == "N": 00097 print "Done collecting far samples" 00098 keep_collecting = False 00099 else: 00100 for cur_sample_path in full_paths: 00101 print "On far sample [%s]" % cur_sample_path 00102 cur_config = yaml.load(open(cur_sample_path)) 00103 m_robot = executive.capture(cur_config, rospy.Duration(40)) 00104 if m_robot is None: 00105 print "--------------- Failed To Capture a Far Sample -----------------" 00106 else: 00107 print "++++++++++++++ Successfully Captured a Far Sample ++++++++++++++" 00108 far_success_count += 1 00109 pub.publish(m_robot) 00110 print "Succeeded on %u far samples" % far_success_count 00111 if rospy.is_shutdown(): 00112 break 00113 00114 # Capture Left Arm Data 00115 if capture_left_arm_data and not rospy.is_shutdown(): 00116 full_paths = [samples_dir + "/left/" + x for x in left_sample_names] 00117 00118 cur_config = yaml.load(open(full_paths[0])) 00119 m_robot = executive.capture(cur_config, rospy.Duration(0.01)) 00120 00121 print "Please put the checkerboard in the left hand (open/close the gripper with the joystick's left/right D-Pad buttons). press <enter> to continue. Type N to skip" 00122 resp = raw_input("press <enter> ") 00123 if string.upper(resp) == "N": 00124 print "Skipping left arm samples" 00125 else: 00126 for cur_sample_path in full_paths: 00127 print "On left arm sample [%s]" % cur_sample_path 00128 cur_config = yaml.load(open(cur_sample_path)) 00129 m_robot = executive.capture(cur_config, rospy.Duration(40)) 00130 if m_robot is None: 00131 print "--------------- Failed To Capture a Left Hand Sample -----------------" 00132 left_fail_count += 1 00133 else: 00134 print "++++++++++++++ Successfully Captured a Left Hand Sample ++++++++++++++" 00135 left_success_count += 1 00136 pub.publish(m_robot) 00137 print "Succeded on %u/%u left arm samples" % (left_success_count, left_fail_count + left_success_count) 00138 if rospy.is_shutdown(): 00139 break 00140 00141 # Capture Right Arm Data 00142 if capture_right_arm_data and not rospy.is_shutdown(): 00143 full_paths = [samples_dir + "/right/" + x for x in right_sample_names] 00144 00145 cur_config = yaml.load(open(full_paths[0])) 00146 m_robot = executive.capture(cur_config, rospy.Duration(0.01)) 00147 00148 print "Please put the checkerboard in the right hand (open/close the gripper with the joystick's square/circle buttons). press <enter> to continue..." 00149 resp = raw_input("press <enter> ") 00150 if string.upper(resp) == "N": 00151 print "Skipping right arm samples" 00152 else: 00153 for cur_sample_path in full_paths: 00154 print "On right arm sample [%s]" % cur_sample_path 00155 cur_config = yaml.load(open(cur_sample_path)) 00156 m_robot = executive.capture(cur_config, rospy.Duration(40)) 00157 if m_robot is None: 00158 print "--------------- Failed To Capture a Right Hand Sample -----------------" 00159 right_fail_count += 1 00160 else: 00161 print "++++++++++++++ Successfully Captured a Right Hand Sample ++++++++++++++" 00162 right_success_count += 1 00163 pub.publish(m_robot) 00164 print "Succeded on %u/%u right arm samples" % (right_success_count, right_fail_count + right_success_count) 00165 if rospy.is_shutdown(): 00166 break 00167 00168 except EOFError: 00169 print "Exiting" 00170 00171 time.sleep(1) 00172 00173 print "Calibration data collection has completed!" 00174 print "Far Samples: %u" % far_success_count 00175 print "Left Arm Samples: %u/%u" % (left_success_count, left_fail_count + left_success_count) 00176 print "Right Arm Samples: %u/%u" % (right_success_count, right_fail_count + right_success_count) 00177 print "" 00178 print "You can now kill this node, along with any other calibration nodes that are running."