class SegmentGrabcut(smach.State):
def __init__(self):
smach.State.__init__(self,
outcomes=[
'done_segmenting', 'segmentation_failed',
'restart_segmentation', 'no_objects'
],
output_keys=['object_points', 'object_name'])
self.sensors = SensorHandler()
self.scene = SceneHandler()
def execute(self, userdata):
# we're willing to wait 15seconds to get the data
kinect_data = self.sensors.get_kinect(15)
if not kinect_data:
return 'segmentation_failed'
(state, result) = self.sensors.get_segmentation(kinect_data)
if state != GoalStatus.SUCCEEDED:
return 'segmentation_failed'
points = self.sensors.get_point_cloud(kinect_data, result.mask)
if not points:
return 'segmentation_failed'
userdata.object_points = points
userdata.object_name = 'obj1'
# need to set up the planning scene ...
#self.scene.clear_scene()
#self.scene.add_table()
#self.scene.add_object('obj1', points)
self.scene.update_scene(points)
rospy.loginfo('... sleeping')
#rospy.sleep(5.0)
print 'successful segmentation!'
return 'done_segmenting'
