def recognize_objects(graspable_objects):
req = GetClusterLabelsRequest()
req.clusters = [go.cluster for go in graspable_objects]
rospy.loginfo('waiting for /get_cluster_labels')
rospy.wait_for_service('/get_cluster_labels')
rospy.loginfo('done')
rec_srv = rospy.ServiceProxy('/get_cluster_labels', GetClusterLabels)
rospy.loginfo('created service proxy')
res = rec_srv(req)
rospy.loginfo('got result')
return res.labels
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
