class Segment(smach.State):
     def __init__(self):
         smach.State.__init__(self, outcomes=['done_segmenting', 'segmentation_failed', 'restart_segmentation'],
                              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)
         
         print 'successful segmentation!'
         return 'done_segmenting'
Esempio n. 2
0
    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()
Esempio n. 3
0
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'
     def __init__(self):
         smach.State.__init__(self, outcomes=['done_segmenting', 'segmentation_failed', 'restart_segmentation'],
                              output_keys=['object_points', 'object_name'])

         self.sensors = SensorHandler()
         self.scene = SceneHandler()