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()
class Drop(smach.State): def __init__(self): smach.State.__init__(self, outcomes=["drop_done"], input_keys=["object_name"]) self.drop = DropHandler() self.scene = SceneHandler() def execute(self, userdata): self.drop.run_drop(userdata.object_name) self.scene.remove_object(userdata.object_name) return "drop_done"
class Drop(smach.State): def __init__(self): smach.State.__init__(self, outcomes=['drop_done'], input_keys=['object_name']) self.drop = DropHandler() self.scene = SceneHandler() def execute(self, userdata): self.drop.run_drop(userdata.object_name) self.scene.remove_object(userdata.object_name) return 'drop_done'
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() self.orkClient = actionlib.SimpleActionClient('ork_tabletop', TabletopAction) self.point_pub = rospy.Publisher('object_points', PointCloud2)
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() self.orkClient = actionlib.SimpleActionClient("ork_tabletop", TabletopAction)
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'
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'
class Segment(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() self.orkClient = actionlib.SimpleActionClient("ork_tabletop", TabletopAction) def execute(self, userdata): self.orkClient.wait_for_server() goal = TabletopGoal() self.orkClient.send_goal(goal) self.orkClient.wait_for_result() state = self.orkClient.get_state() result = self.orkClient.get_result() if state != GoalStatus.SUCCEEDED: return "segmentation_failed" num_objs = len(result.objects) if num_objs == 0: return "no_objects" obj_idx = random.randrange(0, num_objs) points = result.objects[obj_idx] userdata.object_points = points userdata.object_name = "obj1" # need to set up the planning scene ... self.scene.clear_scene() print result.table_pose print result.table_dims self.scene.add_table(result.table_pose, result.table_dims) self.scene.add_object("obj1", points) print "successful segmentation!" return "done_segmenting"
def __init__(self): smach.State.__init__(self, outcomes=["drop_done"], input_keys=["object_name"]) self.drop = DropHandler() self.scene = SceneHandler()
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()
class SegmentORK(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() self.orkClient = actionlib.SimpleActionClient('ork_tabletop', TabletopAction) self.point_pub = rospy.Publisher('object_points', PointCloud2) def execute(self, userdata): self.orkClient.wait_for_server() goal = TabletopGoal() self.orkClient.send_goal(goal) self.orkClient.wait_for_result() state = self.orkClient.get_state() result = self.orkClient.get_result() if state != GoalStatus.SUCCEEDED: return 'segmentation_failed' num_objs = len(result.objects) if num_objs == 0: return 'no_objects' # need to set up the planning scene ... self.scene.clear_scene() print result.table_pose print result.table_dims self.scene.add_table(result.table_pose, result.table_dims) # randomly choose object to pick up obj_idx = random.randrange(0, num_objs) points = result.objects[obj_idx] userdata.object_points = points pick_object = 'obj' + str(obj_idx) userdata.object_name = pick_object self.scene.add_object(pick_object, points) #rospy.sleep(5.0) #self.point_pub.publish(result.objects[obj_idx]) #print obj_idx # and, add only the other objects to the octomap #for idx in range(num_objs): #if idx != obj_idx: # print 'publishing object %d' % idx # self.point_pub.publish(result.objects[idx]) # rospy.sleep(2.0) #obj_name = 'obj' + str(idx) #obj_points = result.objects[idx] #self.scene.add_object(obj_name, obj_points) print 'successful segmentation!' return 'done_segmenting'
def __init__(self): smach.State.__init__(self, outcomes=['drop_done'], input_keys=['object_name']) self.drop = DropHandler() self.scene = SceneHandler()