def call_collision_map_processing( self, detection_result, reset_collision_models=True, reset_attached_models=True, desired_frame="/base_link" ): """Calls the collision map processing service. Given the result of detect, adds the objects to the collision map and returns them as instances of GraspableObjects with correct collision names. Parameters: detection_result: a tabletop_object_detector/TabletopDetectionResponse msg, usually returned by detect(). reset_collision_models: whether the current collision models should be reset before adding new models reset_attached_models: whether the current list of models attached to the robot should be reset desired_frame: what tf frame the results should be returned in """ if detection_result is None: rospy.logerr("Error: using a None detection_result") return None rospy.loginfo("Calling collision map processing") processing_call = TabletopCollisionMapProcessingRequest() processing_call.detection_result = detection_result.detection processing_call.reset_attached_models = reset_attached_models processing_call.reset_collision_models = reset_collision_models processing_call.desired_frame = desired_frame try: self.last_collision_processing_msg = self.collision_processing.call(processing_call) except ServiceException, e: rospy.logerr("Error calling collision map: %s" % str(e)) self.last_collision_processing_msg = None
def update_collision_map(self, tabletop_detection_result): req = TabletopCollisionMapProcessingRequest() req.detection_result = tabletop_detection_result req.reset_collision_models = True req.reset_attached_models = True req.desired_frame = 'base_link' res = self.collision_map_processing_srv(req) rospy.loginfo('found %d clusters: %s, surface name: %s' % (len(res.graspable_objects), res.collision_object_names, res.collision_support_surface_name)) return res
def update_collision_map(self, tabletop_detection_result): rospy.loginfo('collision_map update in progress') req = TabletopCollisionMapProcessingRequest() req.detection_result = tabletop_detection_result req.reset_collision_models = True req.reset_attached_models = True req.desired_frame = 'base_link' res = self.collision_map_processing_srv(req) rospy.loginfo('collision_map update is done') rospy.loginfo('there are %d graspable objects %s, collision support surface name is "%s"' % (len(res.graspable_objects), res.collision_object_names, res.collision_support_surface_name)) return res
def update_collision_map(self, tabletop_detection_result): rospy.loginfo('collision_map update in progress') req = TabletopCollisionMapProcessingRequest() req.detection_result = tabletop_detection_result req.reset_collision_models = True req.reset_attached_models = True req.desired_frame = 'base_link' res = self.collision_map_processing_srv(req) rospy.loginfo('collision_map update is done') rospy.loginfo( 'there are %d graspable objects %s, collision support surface name is "%s"' % (len(res.graspable_objects), res.collision_object_names, res.collision_support_surface_name)) return res
def call_collision_map_processing(self, detection_result): if detection_result is None: rospy.logerr("Error: using a None detection_result") return None rospy.loginfo("Calling collision map processing") processing_call = TabletopCollisionMapProcessingRequest() processing_call.detection_result = detection_result.detection # ask for the exising map and collision models to be reset processing_call.reset_attached_models = True processing_call.reset_collision_models = True processing_call.desired_frame = "base_link" try: self.last_collision_processing_msg = self.collision_processing.call(processing_call) except ServiceException, e: rospy.logerr("Error calling collision map: %s" % str(e)) self.last_collision_processing_msg = None
def call_collision_map_processing(self, detection_result): if detection_result is None: rospy.logerr("Error: using a None detection_result") return None rospy.loginfo("Calling collision map processing") processing_call = TabletopCollisionMapProcessingRequest() processing_call.detection_result = detection_result.detection # ask for the exising map and collision models to be reset processing_call.reset_attached_models = True processing_call.reset_collision_models = True processing_call.desired_frame = "base_link" try: self.last_collision_processing_msg = self.collision_processing.call( processing_call) except ServiceException, e: rospy.logerr("Error calling collision map: %s" % str(e)) self.last_collision_processing_msg = None
tdr = TabletopDetectionResult() tdr.table = res.table tdr.clusters = res.clusters tdr.result = res.result update_collision_map = rospy.ServiceProxy( '/tabletop_collision_map_processing/tabletop_collision_map_processing', TabletopCollisionMapProcessing) req = TabletopCollisionMapProcessingRequest() req.detection_result = tdr req.reset_collision_models = True req.reset_attached_models = True req.reset_static_map = True req.take_static_collision_map = True req.desired_frame = 'base_link' coll_map_res = update_collision_map(req) rospy.loginfo('found %d clusters, grabbing first' % len(coll_map_res.graspable_objects)) #obj_pcluster = res.clusters[0] req = PickupGoal() req.arm_name = 'left_arm' req.target = coll_map_res.graspable_objects[0] #req.target.type = GraspableObject.POINT_CLUSTER #req.target.cluster = obj_pcluster req.desired_approach_distance = 0.05 req.min_approach_distance = 0.03 req.lift.direction.header.frame_id = 'base_link'
def pick_up(self, whicharm="l", NUM_TRIES=5, near_pose=None): self.pickup_client.cancel_goal() for i in range(NUM_TRIES): rospy.loginfo("Attempt %d to grasp " % i ) # call the tabletop detection rospy.loginfo("Calling tabletop detector") detection_call = TabletopDetectionRequest() # we want recognized database objects returned # set this to false if you are using the pipeline without the database detection_call.return_clusters = True #we want the individual object point clouds returned as well detection_call.return_models = True detection_call.num_models = 1 detection_call_response = self.object_detection_srv(detection_call).detection if not detection_call_response: rospy.loginfo("Tabletop detection service failed") success=False; break if detection_call_response.result != detection_call_response.SUCCESS: rospy.loginfo("Tabletop detection returned error code %d", detection_call_response.result) success=False; break if len(detection_call_response.clusters)==0\ and len(detection_call_response.models)==0 : rospy.loginfo("The tabletop detector detected the table, " "but found no objects") success=False; break # call collision map processing rospy.loginfo("Calling collision map processing") processing_call = TabletopCollisionMapProcessingRequest() # pass the result of the tabletop detection processing_call.detection_result = detection_call_response print detection_call_response.models import pdb;pdb.set_trace() # ask for the existing map and collision models to be reset processing_call.reset_collision_models = True processing_call.reset_attached_models = True #ask for the results to be returned in base link frame processing_call.desired_frame = "base_link" processing_call_response = self.collision_processing_srv(processing_call) if not processing_call_response: rospy.loginfo("Collision map processing service failed") success=False; break #the collision map processor returns instances of graspable objects if len(processing_call_response.graspable_objects) == 0: rospy.loginfo("Collision map processing returned no graspable objects") success=False; break graspable_objects = processing_call_response.graspable_objects # sort graspable objects import pdb; # sort graspable objects objects_with_offsets = [None]*len(graspable_objects) #near_point = np.array(list(near_pose[:3]) + [1.0]) for i, obj in enumerate(graspable_objects): pdb.set_trace() points = point_cloud_to_mat(obj.cluster) #centroid =np.reshape(np.mean(points, axis=1), 4) - near_point #offset = np.linalg.norm(centroid - near_point) #objects_with_offsets[i] = (offset,obj) #ordering = sorted(objects_with_offsets, key=lambda item:float((item[0]))) #graspable_objects = [items[1] for items in ordering] #print [items[0] for items in ordering] for i, grasp_object in enumerate(graspable_objects): # call object pickup rospy.loginfo("Calling the pickup action"); pickup_goal = PickupGoal() # pass one of the graspable objects returned # by the collision map processor #pickup_goal.target = processing_call_response.graspable_objects[0] pickup_goal.target = grasp_object # pass the name that the object has in the collision environment # this name was also returned by the collision map processor pickup_goal.collision_object_name = \ processing_call_response.collision_object_names[i] # pass the collision name of the table, also returned by the collision # map processor pickup_goal.collision_support_surface_name = \ processing_call_response.collision_support_surface_name # pick up the object with the left arm if whicharm=="l": pickup_goal.arm_name = "left_arm" else: pickup_goal.arm_name = "right_arm" # we will be lifting the object along the "vertical" direction # which is along the z axis in the base_link frame direction = Vector3Stamped() direction.header.stamp = rospy.Time.now() direction.header.frame_id = "base_link" direction.vector.x = 0 direction.vector.y = 0 direction.vector.z = 1 pickup_goal.lift.direction = direction #request a vertical lift of 10cm after grasping the object pickup_goal.lift.desired_distance = 0.1; pickup_goal.lift.min_distance = 0.05; #do not use tactile-based grasping or tactile-based lift pickup_goal.use_reactive_lift = False; pickup_goal.use_reactive_execution = False; #pickup_goal.allow_gripper_support_collision = True #pickup_goal.ignore_collisions = True #send the goal rospy.loginfo("Waiting for the pickup action...") self.pickup_client.send_goal_and_wait(pickup_goal,rospy.Duration(15)) pickup_result =self.pickup_client.get_result() assert isinstance(pickup_result, PickupResult) success = pickup_result.manipulation_result.value ==\ pickup_result.manipulation_result.SUCCESS if success: rospy.loginfo("Success! Grasped Object.") return success if not success: rospy.loginfo("failed to grasp object") return success
rospy.logerr('TabletopSegmentation did not find any clusters') exit(1) tdr = TabletopDetectionResult() tdr.table = res.table tdr.clusters = res.clusters tdr.result = res.result update_collision_map = rospy.ServiceProxy('/tabletop_collision_map_processing/tabletop_collision_map_processing', TabletopCollisionMapProcessing) req = TabletopCollisionMapProcessingRequest() req.detection_result = tdr req.reset_collision_models = True req.reset_attached_models = True req.reset_static_map = True req.take_static_collision_map = True req.desired_frame = 'base_link' coll_map_res = update_collision_map(req) rospy.loginfo('found %d clusters, grabbing first' % len(coll_map_res.graspable_objects)) #obj_pcluster = res.clusters[0] req = PickupGoal() req.arm_name = 'left_arm' req. target = coll_map_res.graspable_objects[0] #req.target.type = GraspableObject.POINT_CLUSTER #req.target.cluster = obj_pcluster req.desired_approach_distance = 0.05 req.min_approach_distance = 0.03 req.lift.direction.header.frame_id = 'base_link' req.lift.direction.vector.x = 0
collision map processing ''' rospy.loginfo("processing collision map...") # set up collision map request col_req = TabletopCollisionMapProcessingRequest() # pass the result of the tabletop detection col_req.detection_result = det_res.detection # ask for the existing map and collision models to be reset col_req.reset_static_map = 1 col_req.reset_collision_models = 1 col_req.reset_attached_models = 1 # ask for a new static collision map to be taken with the laser after the new models are added to the environment col_req.take_static_collision_map = 1 # ask for the results to be returned in base_link frame col_req.desired_frame = '/base_link' # call collision map processing to add the detected objects to the collision map and get back a list of GraspableObjects try: col_res = collision_map_processing_srv(col_req) except rospy.ServiceException, e: rospy.logerr("error when calling %s: %s" % (collision_map_processing_name, e)) sys.exit() # print out detected objects for (ind, object) in enumerate(col_res.graspable_objects): if object.type == GraspableObject.DATABASE_MODEL: rospy.loginfo("object %d: recognized object with id %d" % (ind, object.model_pose.model_id)) else:
sys.exit() rospy.loginfo("%d objects detected" % len(detection_reply.detection.clusters)) # detection_reply.detection.cluster_model_indices = tuple(xrange(len(detection_reply.detection.clusters))) rospy.loginfo("Calling collision map processing") processing_call = TabletopCollisionMapProcessingRequest() processing_call.detection_result = detection_reply.detection # ask for the exising map and collision models to be reset processing_call.reset_attached_models = False processing_call.reset_collision_models = False processing_call.reset_static_map = False # after the new models are added to the environment processing_call.take_static_collision_map = False processing_call.desired_frame = "base_link" processing_reply = collision_processing_srv.call(processing_call) if len(processing_reply.graspable_objects) == 0: rospy.logerr("Collision map processing returned no graspable objects") rospy.signal_shutdown("") sys.exit() sys.exit() rospy.loginfo("Calling Pickup") max_len = 0 index = 0 for i, c in enumerate(detection_reply.detection.clusters): if len(c.points) > max_len:
rospy.signal_shutdown("") sys.exit() rospy.loginfo("%d objects detected" % len(detection_reply.detection.clusters)) # detection_reply.detection.cluster_model_indices = tuple(xrange(len(detection_reply.detection.clusters))) rospy.loginfo("Calling collision map processing") processing_call = TabletopCollisionMapProcessingRequest() processing_call.detection_result = detection_reply.detection # ask for the exising map and collision models to be reset processing_call.reset_attached_models = False processing_call.reset_collision_models = False processing_call.reset_static_map = False # after the new models are added to the environment processing_call.take_static_collision_map = False processing_call.desired_frame = "base_link" processing_reply = collision_processing_srv.call(processing_call) if len(processing_reply.graspable_objects) == 0: rospy.logerr("Collision map processing returned no graspable objects") rospy.signal_shutdown("") sys.exit() sys.exit() rospy.loginfo("Calling Pickup") max_len = 0 index = 0 for i,c in enumerate(detection_reply.detection.clusters): if len(c.points) > max_len: