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 reset_collision_map(self):
req = TabletopCollisionMapProcessingRequest()
req.detection_result = TabletopDetectionResult()
req.reset_collision_models = True
req.reset_attached_models = True
self.collision_map_processing_srv(req)
rospy.loginfo('collision map reset')
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):
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
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
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'
rospy.wait_for_service('/tabletop_segmentation')
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)
collision_map_processing_srv = rospy.ServiceProxy('/tabletop_collision_map_processing/tabletop_collision_map_processing', TabletopCollisionMapProcessing)
rospy.loginfo('connected to tabletop_collision_map_processing service')
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')
