def segmentationReplyToDetectionReply(self, segmentationResult): '''Converts a TableTopSegmentationReply into a TabletopDetectionResult''' detectionResult = TabletopDetectionResult() detectionResult.table = segmentationResult.table detectionResult.clusters = segmentationResult.clusters detectionResult.models.append(DatabaseModelPoseList()) for i in range(len(detectionResult.clusters)): detectionResult.cluster_model_indices.append(0) detectionResult.result = segmentationResult.result return detectionResult
def segment_objects(self, req): segmentation_result = self.tabletop_segmentation_srv() if segmentation_result.result != TabletopSegmentationResponse.SUCCESS or not segmentation_result.clusters: rospy.logerr('TabletopSegmentation did not find any clusters') return None rospy.loginfo('TabletopSegmentation found %d clusters' % len(segmentation_result.clusters)) tdr = TabletopDetectionResult() tdr.table = segmentation_result.table tdr.clusters = segmentation_result.clusters tdr.result = segmentation_result.result req = GetClusterLabelsRequest() req.clusters = segmentation_result.clusters res = self.classify_srv(req) labels = res.labels index_to_label = { 0: 'Lego', 1: 'Box', 2: 'Toy', 3: 'Ball', 4: 'Cup', } label_to_index = { 'Lego': 0, 'Box': 1, 'Toy': 2, 'Ball': 3, 'Cup': 4, } for idx, cluster in enumerate(segmentation_result.clusters): bbox_response = self.find_bounding_box_srv(cluster) dmp = DatabaseModelPose() dmp.model_id = label_to_index[labels[idx]] dmp.pose = bbox_response.pose dmpl = DatabaseModelPoseList() dmpl.model_list = [dmp] tdr.models.append(dmpl) res = TabletopDetectionResponse() res.detection = tdr rospy.loginfo(res) return res
def segment_objects(self, req): segmentation_result = self.tabletop_segmentation_srv() if segmentation_result.result != TabletopSegmentationResponse.SUCCESS or not segmentation_result.clusters: rospy.logerr('TabletopSegmentation did not find any clusters') return None rospy.loginfo('TabletopSegmentation found %d clusters' % len(segmentation_result.clusters)) tdr = TabletopDetectionResult() tdr.table = segmentation_result.table tdr.clusters = segmentation_result.clusters tdr.result = segmentation_result.result req = GetClusterLabelsRequest() req.clusters = segmentation_result.clusters res = self.classify_srv(req) labels = res.labels index_to_label = { 0: 'Lego', 1: 'Box', 2: 'Toy', 3: 'Ball', 4: 'Cup', } label_to_index = { 'Lego': 0, 'Box': 1, 'Toy': 2, 'Ball': 3, 'Cup': 4, } for idx,cluster in enumerate(segmentation_result.clusters): bbox_response = self.find_bounding_box_srv(cluster) dmp = DatabaseModelPose() dmp.model_id = label_to_index[labels[idx]] dmp.pose = bbox_response.pose dmpl = DatabaseModelPoseList() dmpl.model_list = [dmp] tdr.models.append(dmpl) res = TabletopDetectionResponse() res.detection = tdr rospy.loginfo(res) return res
def execute(self): res = self.object_detector.detect() if res is None: return None segmentation_result = res['segmentation_result'] tdr = TabletopDetectionResult() tdr.table = segmentation_result.table tdr.clusters = segmentation_result.clusters tdr.result = segmentation_result.result tcmpr = self.update_collision_map(tdr) return (tcmpr, res)
def execute(self): res = self.object_detector.detect() if res is None: return None segmentation_result = res['segmentation_result'] tdr = TabletopDetectionResult() tdr.table = segmentation_result.table tdr.clusters = segmentation_result.clusters tdr.result = segmentation_result.result tcmpr = self.update_collision_map(tdr) return (tcmpr, res['cluster_information'])
def execute(self, userdata): res = self.object_detector.detect() if res is None: return 'no_clusters' segmentation_result = res['segmentation_result'] tdr = TabletopDetectionResult() tdr.table = segmentation_result.table tdr.clusters = segmentation_result.clusters tdr.result = segmentation_result.result tcmpr = self.update_collision_map(tdr) userdata.segmentation_result = tcmpr userdata.cluster_information = res['cluster_information'] return 'found_clusters'
def segment_objects(self): res = self.object_detector.detect() if res is None: rospy.logerr('TabletopSegmentation did not find any clusters') return None segmentation_result = res['segmentation_result'] self.info = res['cluster_information'] tdr = TabletopDetectionResult() tdr.table = segmentation_result.table tdr.clusters = segmentation_result.clusters tdr.result = segmentation_result.result tcmpr = self.update_collision_map(tdr) return tcmpr
def segment_objects(self): """ Performs tabletop segmentation. If successful, returns a TabletopDetectionResult objct ready to be passed for processing to tabletop collision map processing node. """ segmentation_result = self.tabletop_segmentation_srv() #rospy.loginfo(segmentation_result) #rospy.loginfo(TabletopSegmentationResponse.SUCCESS) if segmentation_result.result != TabletopSegmentationResponse.SUCCESS or not segmentation_result.clusters: rospy.logerr('TabletopSegmentation did not find any clusters') return None rospy.loginfo('TabletopSegmentation found %d clusters' % len(segmentation_result.clusters)) tdr = TabletopDetectionResult() tdr.table = segmentation_result.table tdr.clusters = segmentation_result.clusters tdr.result = segmentation_result.result return tdr
object_manipulator = SimpleActionClient( 'object_manipulator/object_manipulator_pickup', PickupAction) object_manipulator.wait_for_server() print 'got object_manipulator/object_manipulator_pickup' get_obj = rospy.ServiceProxy('/tabletop_segmentation', TabletopSegmentation) res = get_obj() if res.result != 4 or len(res.clusters) == 0: 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)
print 'got tabletop_segmentation' object_manipulator = SimpleActionClient('object_manipulator/object_manipulator_pickup', PickupAction) object_manipulator.wait_for_server() print 'got object_manipulator/object_manipulator_pickup' get_obj = rospy.ServiceProxy('/tabletop_segmentation', TabletopSegmentation) res = get_obj() if res.result != 4 or len(res.clusters) == 0: 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]
rospy.wait_for_service('/GraspPlanning') grasp_planning_srv = rospy.ServiceProxy('/GraspPlanning', GraspPlanning) rospy.loginfo('connected to GraspPlanning service') segmentation_result = tabletop_segmentation_srv() if segmentation_result.result != TabletopSegmentationResponse.SUCCESS or len(segmentation_result.clusters) == 0: rospy.logerr('TabletopSegmentation did not find any clusters') exit(1) rospy.loginfo('TabletopSegmentation found %d clusters, will update collision map now' % len(segmentation_result.clusters)) tdr = TabletopDetectionResult() tdr.table = segmentation_result.table tdr.clusters = segmentation_result.clusters tdr.result = segmentation_result.result req = TabletopCollisionMapProcessingRequest() req.detection_result = tdr req.reset_collision_models = True req.reset_attached_models = True req.reset_static_map = False req.take_static_collision_map = False req.desired_frame = 'base_link' coll_map_res = 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(coll_map_res.graspable_objects), coll_map_res.collision_object_names, coll_map_res.collision_support_surface_name))