pr2_sample_chooser.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 import roslib; roslib.load_manifest('pr2_calibration_launch')
00035 import rospy
00036 import yaml
00037 import sys
00038 import capture_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 = capture_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     # Capture Far Checkerboards
00080     keep_collecting = True
00081 
00082     while not rospy.is_shutdown() and keep_collecting:
00083         print "Choose L,R, or F"
00084         resp = raw_input("> ")
00085         if string.upper(resp) == "L":
00086             print "Choose sample index"
00087             resp = raw_input("> ")
00088             cur_sample_path = samples_dir + "/left/" + left_sample_names[int(resp)]
00089         elif string.upper(resp) == "R":
00090             print "Choose sample index"
00091             resp = raw_input("> ")
00092             cur_sample_path = samples_dir + "/right/" + right_sample_names[int(resp)]
00093         elif string.upper(resp) == "F":
00094             cur_sample_path = samples_dir + "/far/" + far_sample_names[0]
00095         elif string.upper(resp) == "":
00096             print "Repeating Previous Command"
00097         else:
00098             print "Unknown input. Exiting"
00099             break
00100 
00101         print "On sample [%s]" % cur_sample_path
00102         cur_config = yaml.load(open(cur_sample_path))
00103 
00104         print "Choose Timeout"
00105         timeout = raw_input("> ")
00106         if timeout == "":
00107             timeout = "1.0"
00108 
00109         m_robot = executive.capture(cur_config, rospy.Duration(float(timeout)))
00110         if m_robot is None:
00111             print "--------------- Failed To Capture a Far Sample -----------------"
00112         else:
00113             print "++++++++++++++ Successfully Captured a Far Sample ++++++++++++++"
00114             pub.publish(m_robot)
00115 
00116 
00117 except EOFError:
00118     print "Exiting"
00119 


pr2_calibration_launch
Author(s): Vijay Pradeep
autogenerated on Fri Aug 28 2015 12:06:40