pr2_se_exec.py
Go to the documentation of this file.
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."
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends


pr2_se_calibration_launch
Author(s): Adam Leeper, Vijay Pradeep
autogenerated on Mon Aug 19 2013 10:50:53