$search
00001 #!/usr/bin/python 00002 # Software License Agreement (BSD License) 00003 # 00004 # Copyright (c) 2009, 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 the Willow Garage 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 # author: Kaijen Hsiao 00035 00036 ## @package pick_and_place_demo 00037 #Pick and place demo: picks objects up from one side of the table and moves 00038 #them to the other, then switches sides 00039 00040 import roslib 00041 roslib.load_manifest('pr2_pick_and_place_demos') 00042 import rospy 00043 from pr2_pick_and_place_demos.pick_and_place_manager import * 00044 00045 00046 ##class for running the pick and place demo, inherits from PickAndPlaceManager 00047 class PickAndPlaceDemo(PickAndPlaceManager): 00048 00049 00050 def __init__(self, use_slip_controller = 0, use_slip_detection = 0): 00051 00052 #run the standard init 00053 PickAndPlaceManager.__init__(self, use_slip_controller, use_slip_detection) 00054 00055 #which side we're getting objects from (right = 0, left = 1, placing is on the other side) 00056 self.pick_up_side = 0 00057 self.put_down_side = 1 00058 00059 #(x,y) dimensions of each table area (side) that we want to place/pick up from 00060 self.table_side_dims = [.3, .3] 00061 00062 #middle line separating right and left (base_link y) 00063 self.middle_line = 0. 00064 00065 #center of each table side in '/base_link' frame (0=right, 1=left, height and dist from table added later) 00066 self.table_center_offsets = [[self.table_side_dims[1]/2, -(self.table_side_dims[0]/2+.02)+self.middle_line, 0], 00067 [self.table_side_dims[1]/2, self.table_side_dims[0]/2+.02+self.middle_line, 0]] 00068 00069 #temporary table info until we get a table detection 00070 #centers of the table halves 00071 self.table_centers = copy.deepcopy(self.table_center_offsets) 00072 for i in range(2): 00073 self.table_centers[i][0] += self.table_front_edge_x + .05 00074 self.table_centers[i][2] += self.table_height 00075 00076 #update where to point the head and draw the table centers 00077 self.update_head_point_locs() 00078 00079 #set the place rectangle to the initial put-down side 00080 self.set_table_place_rectangle(self.put_down_side) 00081 00082 #are we trying to preempt the autonomous thread? 00083 self.preempting = 0 00084 00085 #lock for self.preempting and variable to keep track of whether we're in the autonomous thread 00086 self.lock = threading.Lock() 00087 self.autonomous_thread_running = 0 00088 00089 #is the autonomous demo going to run in constrained mode or not? 00090 self.constrained = False 00091 00092 00093 ##update where to point the head based on the table centers 00094 def update_head_point_locs(self): 00095 00096 #places to point the head to 00097 self.head_point_locs = copy.deepcopy(self.table_centers) 00098 self.head_point_locs[0][0] += .1 #forward of center 00099 self.head_point_locs[1][0] += .1 00100 #in a bit toward center (add more or less if the center is shifted) 00101 self.head_point_locs[0][1] += .05-self.middle_line/2.1 00102 self.head_point_locs[1][1] -= .05+self.middle_line/1.2 00103 00104 00105 ##extended to update the table side regions 00106 def update_table_info(self, adjust_place_rectangle = 0): 00107 00108 #update self.table_front_edge_x and self.table_height 00109 PickAndPlaceManager.update_table_info(self, adjust_place_rectangle) 00110 00111 #centers of the table halves 00112 self.table_centers = copy.deepcopy(self.table_center_offsets) 00113 for i in range(2): 00114 self.table_centers[i][0] += self.table_front_edge_x + .05 00115 self.table_centers[i][2] += self.table_height 00116 00117 #update where to point the head and draw the table centers 00118 self.update_head_point_locs() 00119 00120 00121 ##overridden to exit from the autonomous thread 00122 def throw_exception(self): 00123 self.exit_autonomous_thread() 00124 00125 00126 ##move objects from one side of the table to the other 00127 def move_objects_to_other_side(self): 00128 00129 #only try each object max_object_tries times 00130 max_object_tries = 5 00131 object_tries = 0 00132 while(not rospy.is_shutdown() and object_tries < max_object_tries): 00133 rospy.loginfo("object_tries:%d", object_tries) 00134 00135 self.check_preempted() 00136 00137 #head point location is the pick-up side 00138 head_point = copy.deepcopy(self.head_point_locs[self.pick_up_side]) 00139 head_point[0] += scipy.rand()*.05 00140 head_point[1] += scipy.rand()*.05 00141 00142 #try the arm corresponding to the pick-up side first, then the other arm (if available) 00143 if self.pick_up_side == 0 and self.arms_to_use == "both": 00144 arms_to_try = [0,1] 00145 elif self.pick_up_side == 1 and self.arms_to_use == "both": 00146 arms_to_try = [1,0] 00147 elif self.arms_to_use == "left": 00148 arms_to_try = [1] 00149 elif self.arms_to_use == "right": 00150 arms_to_try = [0] 00151 00152 #detect and pick up an object 00153 (result, arm_used) = self.detect_and_pick_up_object(head_point, arms_to_try = arms_to_try, constrained = self.constrained) 00154 00155 #no objects left! Quit 00156 if result == 'no objects left': 00157 return 1 00158 00159 #grasp failed. Increment the number of object_tries and try again 00160 if result == 'grasp failed': 00161 object_tries += 1 00162 continue 00163 00164 self.check_preempted() 00165 00166 # #if the object is the bowl, try to transfer it to the other arm before putting it down 00167 # if self.held_objects[arm_used] and self.held_objects[arm_used].object.type == 1 \ 00168 # and self.held_objects[arm_used].object.model_pose.model_id == 18699: 00169 # rospy.loginfo("detected bowl, trying to transfer it to the other hand before putting it down") 00170 # success = self.transfer_object_to_other_hand(arm_used, self.held_objects[arm_used]) 00171 # if success: 00172 # arm_used = 1-arm_used 00173 00174 #success! Reset the number of object_tries 00175 object_tries = 0 00176 00177 #place it on the other side 00178 self.put_down_object(arm_used, use_place_override = 1, constrained = self.constrained) 00179 00180 #ran out of tries 00181 rospy.logerr("ran out of tries to move an object!") 00182 return 0 00183 00184 00185 ##filter/count how many objects in detected_objects are on whichside 00186 def count_objects(self, whichside = None): 00187 if whichside == None: 00188 whichside = self.pick_up_side 00189 00190 object_count = 0 00191 kept_objects = [] 00192 for object in self.detected_objects: 00193 dist = object.pose.pose.position.x-(self.table_centers[0][0]+self.table_side_dims[0]/2.) 00194 if whichside == 0 and object.pose.pose.position.y > self.middle_line: 00195 rospy.loginfo("counting right: ignoring object not on the right side, y pos: %0.3f"%object.pose.pose.position.y) 00196 continue 00197 elif whichside == 1 and object.pose.pose.position.y < self.middle_line: 00198 rospy.loginfo("counting left: ignoring object not on the left side, y pos: %0.3f"%object.pose.pose.position.y) 00199 continue 00200 elif dist > .1: 00201 rospy.loginfo("ignoring object too far away, dist=%5.3f"%dist) 00202 continue 00203 else: 00204 rospy.loginfo("object dist from back edge is %5.3f"%dist) 00205 kept_objects.append(object) 00206 self.detected_objects = kept_objects 00207 00208 return len(self.detected_objects) 00209 00210 00211 ##overridden temporarily to get rid of the single table object when there's nothing on the table 00212 def call_tabletop_detection(self, update_table = 0, clear_attached_objects = 1, \ 00213 replace_table_in_collision_map = 1, update_place_rectangle = 0, \ 00214 num_models = 0): 00215 (detected_objects, table) = PickAndPlaceManager.call_tabletop_detection(self, \ 00216 update_table, clear_attached_objects, \ 00217 replace_table_in_collision_map, update_place_rectangle, num_models) 00218 00219 #remove table object 00220 if len(self.detected_objects) == 1 and self.detected_objects[0].box_dims[0] > .2 and self.detected_objects[0].box_dims[1] > .2: 00221 self.remove_object(self.detected_objects[0].collision_name) 00222 self.detected_objects = [] 00223 rospy.loginfo("removed table object") 00224 00225 return (self.detected_objects, table) 00226 00227 00228 00229 ##choose the side of the table with more objects to be the side to pick them up from 00230 def pick_sides(self): 00231 00232 #figure out which side has more objects 00233 object_counts = [0]*2 00234 for i in range(2): 00235 00236 self.check_preempted() 00237 00238 self.point_head(self.head_point_locs[i], 'base_link') 00239 00240 self.check_preempted() 00241 00242 self.call_tabletop_detection(update_table = 0, clear_attached_objects = 0) 00243 object_counts[i] = self.count_objects(i) 00244 00245 rospy.loginfo("saw %d objects on the right and %d objects on the left"%(object_counts[0], object_counts[1])) 00246 00247 #set the side with more objects to be the side to pick up from 00248 if object_counts[0] > object_counts[1]: 00249 rospy.loginfo("setting pick_up_side to the right side, put_down_side to the left") 00250 self.pick_up_side = 0 00251 self.put_down_side = 1 00252 else: 00253 rospy.loginfo("setting pick_up_side to the left side, put_down_side to the right") 00254 self.pick_up_side = 1 00255 self.put_down_side = 0 00256 00257 #set the place rectangle to the put-down side 00258 self.set_table_place_rectangle(self.put_down_side) 00259 00260 00261 ##switch pick-up and put-down sides 00262 def switch_sides(self): 00263 00264 #switch the flags 00265 self.put_down_side = self.pick_up_side 00266 self.pick_up_side = 1-self.put_down_side 00267 rospy.loginfo("switching sides! pick-up side is now %s, put-down is %s"%(self.arm_dict[self.pick_up_side],\ 00268 self.arm_dict[self.put_down_side])) 00269 00270 #update the place rectangle 00271 self.set_table_place_rectangle(self.put_down_side) 00272 00273 00274 ##set the place rectangle to one side of the table 00275 def set_table_place_rectangle(self, whichside): 00276 00277 rect_pose_mat = scipy.identity(4) 00278 rect_pose_mat[0:3, 3] = scipy.matrix(self.table_centers[whichside]) 00279 rect_pose_stamped = stamp_pose(mat_to_pose(rect_pose_mat), 'base_link') 00280 self.set_place_area(rect_pose_stamped, self.table_side_dims) 00281 00282 00283 ##demo to run continuously in the autonomous thread 00284 def run_demo(self): 00285 00286 #move the arms to the side (out of the way of detection) 00287 for arm in self.arms_to_use_list: 00288 self.move_arm_to_side(arm) 00289 self.check_preempted() 00290 00291 #find the table 00292 self.find_table() 00293 self.check_preempted() 00294 00295 #figure out which side the objects are mostly on 00296 self.pick_sides() 00297 self.check_preempted() 00298 00299 #run continuously until shut down 00300 while not rospy.is_shutdown(): 00301 00302 rospy.loginfo("starting to move objects from %s side to %s side"%(self.arm_dict[self.pick_up_side], \ 00303 self.arm_dict[self.put_down_side])) 00304 result = self.move_objects_to_other_side() 00305 self.check_preempted() 00306 00307 self.switch_sides() 00308 00309 00310 ##check if we want to preempt, and if so, stop and notify the user that we've done so 00311 def check_preempted(self, inside_thread = 1): 00312 preempting = 0 00313 paused = 0 00314 self.lock.acquire() 00315 preempting = self.preempting 00316 self.lock.release() 00317 00318 #preempt requested, exit thread 00319 if preempting == 1 and inside_thread: 00320 rospy.loginfo("saw the preempt") 00321 self.reset_preempted() 00322 self.autonomous_thread_running = 0 00323 sys.exit(0) 00324 00325 #paused, if inside thread, wait for main thread to reset the preempt 00326 if preempting == -1 and inside_thread: 00327 rospy.loginfo("pause received inside thread, press enter to continue or q to exit") 00328 while not rospy.is_shutdown(): 00329 preempting = self.check_preempted(inside_thread = 0) 00330 if not preempting: 00331 break 00332 elif preempting == 1: 00333 rospy.loginfo("quit requested received inside thread, exiting") 00334 self.reset_preempted() 00335 self.autonomous_thread_running = 0 00336 sys.exit(0) 00337 time.sleep(.1) 00338 return 0 00339 00340 return preempting 00341 00342 00343 ##reset the preempt notification 00344 def reset_preempted(self): 00345 rospy.loginfo("resetting the preempt") 00346 self.lock.acquire() 00347 self.preempting = 0 00348 self.lock.release() 00349 00350 00351 ##tell the autonomous thread to start running the demo continuously 00352 def start_autonomous_thread(self,constrained = False): 00353 00354 #start a thread to run the demo continously in a override-able way 00355 self.reset_preempted() 00356 self.constrained = constrained 00357 self.thread = threading.Thread(target=self.run_demo) 00358 self.autonomous_thread_running = 1 00359 self.thread.setDaemon(True) 00360 self.thread.start() 00361 00362 00363 ##stop the autonomous thread by telling it to stop as soon as possible 00364 def stop_autonomous_thread(self): 00365 00366 #tell the autonomous thread to stop as soon as possible 00367 self.lock.acquire() 00368 self.preempting = 1 00369 self.lock.release() 00370 00371 #wait for the preempt to finish 00372 rospy.loginfo("preempt sent, waiting for something to finish") 00373 while not rospy.is_shutdown(): 00374 if not self.check_preempted(inside_thread = 0): 00375 break 00376 time.sleep(.1) 00377 rospy.loginfo("autonomous thread ended") 00378 00379 00380 ##pause the autonomous thread as soon as possible 00381 def pause_autonomous_thread(self): 00382 00383 #tell the autonomous thread to pause as soon as possible 00384 self.lock.acquire() 00385 self.preempting = -1 00386 self.lock.release() 00387 00388 #wait for the pause to finish 00389 rospy.loginfo("pause sent, waiting for something to finish (press enter to continue, q to quit)") 00390 c = raw_input() 00391 if c == 'q': 00392 self.stop_autonomous_thread() 00393 return 1 00394 self.reset_preempted() 00395 rospy.loginfo("pause done") 00396 return 0 00397 00398 00399 ##exit from within the autonomous thread, if running 00400 def exit_autonomous_thread(self): 00401 if self.autonomous_thread_running: 00402 print "saw a bad error, exiting the autonomous thread (hit q to go back to the keyboard interface)" 00403 self.autonomous_thread_running = 0 00404 sys.exit(0) 00405 else: 00406 rospy.logerr("saw a bad error!! Not within autonomous thread; be careful, things may be very broken.") 00407 00408 00409 ##instructions for extensions to the keyboard interface specific to the demo 00410 def print_keyboard_extensions(self): 00411 print "\'start\' to start the autonomous demo" 00412 print "s to switch pick-up and put-down sides" 00413 print "hs to point the head at either side" 00414 00415 00416 ##extensions to the keyboard interface specific to the demo 00417 def keyboard_extensions(self, input): 00418 00419 #run the autonomous thread 00420 if input == 'start': 00421 self.start_autonomous_thread(True) 00422 00423 while not rospy.is_shutdown(): 00424 print "type s to preempt the autonomous demo, p to pause" 00425 input = raw_input() 00426 if input.strip() == 's': 00427 self.stop_autonomous_thread() 00428 break 00429 elif input.strip() == 'p': 00430 exit = self.pause_autonomous_thread() 00431 if exit: 00432 break 00433 elif input.strip() == 'q': 00434 break 00435 time.sleep(0.2) 00436 00437 #switch the pick-up and put-down sides 00438 elif input == 's': 00439 self.switch_sides() 00440 00441 #point the head at either the right or left side 00442 elif input == 'hs': 00443 print "which side? r for right and l for left" 00444 side = self.input_side() 00445 if side != None and side >= 0: 00446 self.point_head(self.head_point_locs[side], 'base_link') 00447 00448 return 0 00449 00450 00451 if __name__ == '__main__': 00452 00453 use_slip_controller = rospy.get_param('/reactive_grasp_node_right/use_slip_controller', False) 00454 use_slip_detection = rospy.get_param('/reactive_grasp_node_right/use_slip_detection', False) 00455 00456 rospy.init_node('pick_and_place_demo', anonymous=True) 00457 pick_and_place_demo = PickAndPlaceDemo(use_slip_controller = use_slip_controller, 00458 use_slip_detection = use_slip_detection) 00459 pick_and_place_demo.keyboard_interface()