00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034 import roslib; roslib.load_manifest('pr2_calibration_launch')
00035 import rospy
00036 import yaml
00037 import sys
00038 import pr2_calibration_executive.capture_exec
00039 import time
00040 from calibration_msgs.msg import RobotMeasurement
00041 import os
00042 import string
00043
00044 print "Starting executive..."
00045 time.sleep(7.0)
00046
00047 rospy.init_node("pr2_capture_executive_node")
00048
00049 samples_dir = rospy.myargv()[1]
00050 config_dir = rospy.myargv()[2]
00051
00052 executive = pr2_calibration_executive.capture_exec.CaptureExecutive(config_dir)
00053 time.sleep(1.0)
00054
00055 left_sample_names = [x for x in os.listdir(samples_dir + "/left/") if ".yaml" in x]
00056 right_sample_names = []
00057 far_sample_names = []
00058 right_sample_names = [x for x in os.listdir(samples_dir + "/right/") if ".yaml" in x]
00059 far_sample_names = [x for x in os.listdir(samples_dir + "/far/") if ".yaml" in x]
00060
00061 left_sample_names.sort()
00062 right_sample_names.sort()
00063 far_sample_names.sort()
00064
00065 print "Left Samples: \n - %s" % "\n - ".join(left_sample_names)
00066 print "Right Samples: \n - %s" % "\n - ".join(right_sample_names)
00067 print "Far Samples: \n - %s" % "\n - ".join(far_sample_names)
00068
00069 pub = rospy.Publisher("robot_measurement", RobotMeasurement)
00070
00071 far_success_count = 0
00072 left_success_count = 0
00073 left_fail_count = 0
00074 right_success_count = 0
00075 right_fail_count = 0
00076
00077
00078 try:
00079
00080 keep_collecting = True
00081 full_paths = [samples_dir + "/far/" + x for x in far_sample_names]
00082 cur_config = yaml.load(open(full_paths[0]))
00083 m_robot = executive.capture(cur_config, rospy.Duration(0.01))
00084
00085 while not rospy.is_shutdown() and keep_collecting:
00086
00087 print "Please place the large 7x6 checkerboard approx 3m in front of the robot"
00088 print "in view of the head cameras and tilting laser."
00089 print "Press <enter> when ready to collect data, or type \"N\" if done collecting large checkerboards"
00090 resp = raw_input("> ")
00091 if string.upper(resp) == "N":
00092 print "Done collecting far samples"
00093 keep_collecting = False
00094 else:
00095 for cur_sample_path in full_paths:
00096 print "On far sample [%s]" % cur_sample_path
00097 cur_config = yaml.load(open(cur_sample_path))
00098 m_robot = executive.capture(cur_config, rospy.Duration(40))
00099 if m_robot is None:
00100 print "--------------- Failed To Capture a Far Sample -----------------"
00101 else:
00102 print "++++++++++++++ Successfully Captured a Far Sample ++++++++++++++"
00103 far_success_count += 1
00104 pub.publish(m_robot)
00105 print "Succeeded on %u far samples" % far_success_count
00106 if rospy.is_shutdown():
00107 break
00108
00109
00110 if not rospy.is_shutdown():
00111 full_paths = [samples_dir + "/left/" + x for x in left_sample_names]
00112
00113 cur_config = yaml.load(open(full_paths[0]))
00114 m_robot = executive.capture(cur_config, rospy.Duration(0.01))
00115
00116 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"
00117 resp = raw_input("press <enter> ")
00118 if string.upper(resp) == "N":
00119 print "Skipping left arm samples"
00120 else:
00121 for cur_sample_path in full_paths:
00122 print "On left arm sample [%s]" % cur_sample_path
00123 cur_config = yaml.load(open(cur_sample_path))
00124 m_robot = executive.capture(cur_config, rospy.Duration(40))
00125 if m_robot is None:
00126 print "--------------- Failed To Capture a Left Hand Sample -----------------"
00127 left_fail_count += 1
00128 else:
00129 print "++++++++++++++ Successfully Captured a Left Hand Sample ++++++++++++++"
00130 left_success_count += 1
00131 pub.publish(m_robot)
00132 print "Succeded on %u/%u left arm samples" % (left_success_count, left_fail_count + left_success_count)
00133 if rospy.is_shutdown():
00134 break
00135
00136
00137 if not rospy.is_shutdown():
00138 full_paths = [samples_dir + "/right/" + x for x in right_sample_names]
00139
00140 cur_config = yaml.load(open(full_paths[0]))
00141 m_robot = executive.capture(cur_config, rospy.Duration(0.01))
00142
00143 print "Please put the checkerboard in the right hand (open/close the gripper with the joystick's square/circle buttons). press <enter> to continue..."
00144 resp = raw_input("press <enter> ")
00145 if string.upper(resp) == "N":
00146 print "Skipping right arm samples"
00147 else:
00148 for cur_sample_path in full_paths:
00149 print "On right arm sample [%s]" % cur_sample_path
00150 cur_config = yaml.load(open(cur_sample_path))
00151 m_robot = executive.capture(cur_config, rospy.Duration(40))
00152 if m_robot is None:
00153 print "--------------- Failed To Capture a Right Hand Sample -----------------"
00154 right_fail_count += 1
00155 else:
00156 print "++++++++++++++ Successfully Captured a Right Hand Sample ++++++++++++++"
00157 right_success_count += 1
00158 pub.publish(m_robot)
00159 print "Succeded on %u/%u right arm samples" % (right_success_count, right_fail_count + right_success_count)
00160 if rospy.is_shutdown():
00161 break
00162
00163 except EOFError:
00164 print "Exiting"
00165
00166 time.sleep(1)
00167
00168 print "Calibration data collection has completed!"
00169 print "Far Samples: %u" % far_success_count
00170 print "Left Arm Samples: %u/%u" % (left_success_count, left_fail_count + left_success_count)
00171 print "Right Arm Samples: %u/%u" % (right_success_count, right_fail_count + right_success_count)
00172 print ""
00173 print "You can now kill this node, along with any other calibration nodes that are running."
