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
00035
00036
00037
00038
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
00047 class PickAndPlaceDemo(PickAndPlaceManager):
00048
00049
00050 def __init__(self, use_slip_controller = 0, use_slip_detection = 0):
00051
00052
00053 PickAndPlaceManager.__init__(self, use_slip_controller, use_slip_detection)
00054
00055
00056 self.pick_up_side = 0
00057 self.put_down_side = 1
00058
00059
00060 self.table_side_dims = [.3, .3]
00061
00062
00063 self.middle_line = 0.
00064
00065
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
00070
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
00077 self.update_head_point_locs()
00078
00079
00080 self.set_table_place_rectangle(self.put_down_side)
00081
00082
00083 self.preempting = 0
00084
00085
00086 self.lock = threading.Lock()
00087 self.autonomous_thread_running = 0
00088
00089
00090 self.constrained = False
00091
00092
00093
00094 def update_head_point_locs(self):
00095
00096
00097 self.head_point_locs = copy.deepcopy(self.table_centers)
00098 self.head_point_locs[0][0] -= .1
00099 self.head_point_locs[1][0] -= .1
00100
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
00106 def update_table_info(self, adjust_place_rectangle = 0):
00107
00108
00109 PickAndPlaceManager.update_table_info(self)
00110
00111
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
00118 self.update_head_point_locs()
00119
00120
00121
00122 def throw_exception(self):
00123 self.exit_autonomous_thread()
00124
00125
00126
00127 def move_objects_to_other_side(self):
00128
00129
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
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
00143 if self.pick_up_side == 0:
00144 arms_to_try = [0,1]
00145 else:
00146 arms_to_try = [1,0]
00147
00148
00149 (result, arm_used) = self.detect_and_pick_up_object(head_point, arms_to_try = arms_to_try, constrained = self.constrained)
00150
00151
00152 if result == 'no objects left':
00153 return 1
00154
00155
00156 if result == 'grasp failed':
00157 object_tries += 1
00158 continue
00159
00160 self.check_preempted()
00161
00162
00163
00164
00165
00166
00167
00168
00169
00170
00171 object_tries = 0
00172
00173
00174 self.put_down_object(arm_used, use_place_override = 1, constrained = self.constrained)
00175
00176
00177 rospy.logerr("ran out of tries to move an object!")
00178 return 0
00179
00180
00181
00182 def count_objects(self, whichside = None):
00183 if whichside == None:
00184 whichside = self.pick_up_side
00185
00186 object_count = 0
00187 kept_objects = []
00188 for object in self.detected_objects:
00189 dist = object.pose.pose.position.x-(self.table_centers[0][0]+self.table_side_dims[0]/2.)
00190 if whichside == 0 and object.pose.pose.position.y > self.middle_line:
00191 rospy.loginfo("counting right: ignoring object not on the right side, y pos: %0.3f"%object.pose.pose.position.y)
00192 continue
00193 elif whichside == 1 and object.pose.pose.position.y < self.middle_line:
00194 rospy.loginfo("counting left: ignoring object not on the left side, y pos: %0.3f"%object.pose.pose.position.y)
00195 continue
00196 elif dist > .1:
00197 rospy.loginfo("ignoring object too far away, dist=%5.3f"%dist)
00198 continue
00199 else:
00200 rospy.loginfo("object dist from back edge is %5.3f"%dist)
00201 kept_objects.append(object)
00202 self.detected_objects = kept_objects
00203
00204 return len(self.detected_objects)
00205
00206
00207
00208 def call_tabletop_detection(self, take_static_collision_map = 0, update_table = 0, clear_attached_objects = 1, \
00209 replace_table_in_collision_map = 1, update_place_rectangle = 0, \
00210 num_models = 0):
00211 (detected_objects, table) = PickAndPlaceManager.call_tabletop_detection(self, take_static_collision_map, \
00212 update_table, clear_attached_objects, \
00213 replace_table_in_collision_map, update_place_rectangle, num_models)
00214
00215
00216 if len(self.detected_objects) == 1 and self.detected_objects[0].box_dims[0] > .2 and self.detected_objects[0].box_dims[1] > .2:
00217 self.remove_object(self.detected_objects[0].collision_name)
00218 self.detected_objects = []
00219 rospy.loginfo("removed table object")
00220
00221 return (self.detected_objects, table)
00222
00223
00224
00225
00226 def pick_sides(self):
00227
00228
00229 object_counts = [0]*2
00230 for i in range(2):
00231
00232 self.check_preempted()
00233
00234 self.point_head(self.head_point_locs[i], 'base_link')
00235
00236 self.check_preempted()
00237
00238 self.call_tabletop_detection(take_static_collision_map = 0, update_table = 0, clear_attached_objects = 0)
00239 object_counts[i] = self.count_objects(i)
00240
00241 rospy.loginfo("saw %d objects on the right and %d objects on the left"%(object_counts[0], object_counts[1]))
00242
00243
00244 if object_counts[0] > object_counts[1]:
00245 rospy.loginfo("setting pick_up_side to the right side, put_down_side to the left")
00246 self.pick_up_side = 0
00247 self.put_down_side = 1
00248 else:
00249 rospy.loginfo("setting pick_up_side to the left side, put_down_side to the right")
00250 self.pick_up_side = 1
00251 self.put_down_side = 0
00252
00253
00254 self.set_table_place_rectangle(self.put_down_side)
00255
00256
00257
00258 def switch_sides(self):
00259
00260
00261 self.put_down_side = self.pick_up_side
00262 self.pick_up_side = 1-self.put_down_side
00263 rospy.loginfo("switching sides! pick-up side is now %s, put-down is %s"%(self.arm_dict[self.pick_up_side],\
00264 self.arm_dict[self.put_down_side]))
00265
00266
00267 self.set_table_place_rectangle(self.put_down_side)
00268
00269
00270
00271 def set_table_place_rectangle(self, whichside):
00272
00273 rect_pose_mat = scipy.identity(4)
00274 rect_pose_mat[0:3, 3] = scipy.matrix(self.table_centers[whichside])
00275 rect_pose_stamped = stamp_pose(mat_to_pose(rect_pose_mat), 'base_link')
00276 self.set_place_area(rect_pose_stamped, self.table_side_dims)
00277
00278
00279
00280 def run_demo(self):
00281
00282
00283 for arm in [0,1]:
00284 self.move_arm_to_side(arm)
00285 self.check_preempted()
00286
00287
00288 self.find_table()
00289 self.check_preempted()
00290
00291
00292 self.pick_sides()
00293 self.check_preempted()
00294
00295
00296 while not rospy.is_shutdown():
00297
00298 rospy.loginfo("starting to move objects from %s side to %s side"%(self.arm_dict[self.pick_up_side], \
00299 self.arm_dict[self.put_down_side]))
00300 result = self.move_objects_to_other_side()
00301 self.check_preempted()
00302
00303 self.switch_sides()
00304
00305
00306
00307 def check_preempted(self, inside_thread = 1):
00308 preempting = 0
00309 paused = 0
00310 self.lock.acquire()
00311 preempting = self.preempting
00312 self.lock.release()
00313
00314
00315 if preempting == 1 and inside_thread:
00316 rospy.loginfo("saw the preempt")
00317 self.reset_preempted()
00318 self.autonomous_thread_running = 0
00319 sys.exit(0)
00320
00321
00322 if preempting == -1 and inside_thread:
00323 rospy.loginfo("pause received inside thread, press enter to continue or q to exit")
00324 while not rospy.is_shutdown():
00325 preempting = self.check_preempted(inside_thread = 0)
00326 if not preempting:
00327 break
00328 elif preempting == 1:
00329 rospy.loginfo("quit requested received inside thread, exiting")
00330 self.reset_preempted()
00331 self.autonomous_thread_running = 0
00332 sys.exit(0)
00333 time.sleep(.1)
00334 return 0
00335
00336 return preempting
00337
00338
00339
00340 def reset_preempted(self):
00341 rospy.loginfo("resetting the preempt")
00342 self.lock.acquire()
00343 self.preempting = 0
00344 self.lock.release()
00345
00346
00347
00348 def start_autonomous_thread(self,constrained = False):
00349
00350
00351 self.reset_preempted()
00352 self.constrained = constrained
00353 self.thread = threading.Thread(target=self.run_demo)
00354 self.autonomous_thread_running = 1
00355 self.thread.setDaemon(True)
00356 self.thread.start()
00357
00358
00359
00360 def stop_autonomous_thread(self):
00361
00362
00363 self.lock.acquire()
00364 self.preempting = 1
00365 self.lock.release()
00366
00367
00368 rospy.loginfo("preempt sent, waiting for something to finish")
00369 while not rospy.is_shutdown():
00370 if not self.check_preempted(inside_thread = 0):
00371 break
00372 time.sleep(.1)
00373 rospy.loginfo("autonomous thread ended")
00374
00375
00376
00377 def pause_autonomous_thread(self):
00378
00379
00380 self.lock.acquire()
00381 self.preempting = -1
00382 self.lock.release()
00383
00384
00385 rospy.loginfo("pause sent, waiting for something to finish (press enter to continue, q to quit)")
00386 c = raw_input()
00387 if c == 'q':
00388 self.stop_autonomous_thread()
00389 return 1
00390 self.reset_preempted()
00391 rospy.loginfo("pause done")
00392 return 0
00393
00394
00395
00396 def exit_autonomous_thread(self):
00397 if self.autonomous_thread_running:
00398 print "saw a bad error, exiting the autonomous thread (hit q to go back to the keyboard interface)"
00399 self.autonomous_thread_running = 0
00400 sys.exit(0)
00401 else:
00402 rospy.logerr("saw a bad error!! Not within autonomous thread; be careful, things may be very broken.")
00403
00404
00405
00406 def print_keyboard_extensions(self):
00407 print "\'start\' to start the autonomous demo"
00408 print "s to switch pick-up and put-down sides"
00409 print "hs to point the head at either side"
00410
00411
00412
00413 def keyboard_extensions(self, input):
00414
00415
00416 if input == 'start':
00417 self.start_autonomous_thread(True)
00418
00419 while not rospy.is_shutdown():
00420 print "type s to preempt the autonomous demo, p to pause"
00421 input = raw_input()
00422 if input.strip() == 's':
00423 self.stop_autonomous_thread()
00424 break
00425 elif input.strip() == 'p':
00426 exit = self.pause_autonomous_thread()
00427 if exit:
00428 break
00429 elif input.strip() == 'q':
00430 break
00431 time.sleep(0.2)
00432
00433
00434 elif input == 's':
00435 self.switch_sides()
00436
00437
00438 elif input == 'hs':
00439 print "which side? r for right and l for left"
00440 side = self.input_side()
00441 if side != None and side >= 0:
00442 self.point_head(self.head_point_locs[side], 'base_link')
00443
00444 return 0
00445
00446
00447 if __name__ == '__main__':
00448
00449 use_slip_controller = rospy.get_param('/reactive_grasp_node_right/use_slip_controller')
00450 use_slip_detection = rospy.get_param('/reactive_grasp_node_right/use_slip_detection')
00451
00452 rospy.init_node('pick_and_place_demo', anonymous=True)
00453 pick_and_place_demo = PickAndPlaceDemo(use_slip_controller = use_slip_controller,
00454 use_slip_detection = use_slip_detection)
00455 pick_and_place_demo.keyboard_interface()