Пример #1
0
 def grasp_goal_cb(userdata, goal):
     grasp_goal = GraspObjectGoal()
     grasp_goal.category_id = -1
     grasp_goal.graspable_object = userdata.segmentation_result.graspable_objects[userdata.closest_index]
     grasp_goal.collision_object_name = userdata.segmentation_result.collision_object_names[userdata.closest_index]
     grasp_goal.collision_support_surface_name = userdata.segmentation_result.collision_support_surface_name
     return grasp_goal
Пример #2
0
def do_it(msg):
    rospy.loginfo('We are DOING IT!')
    
    # connect to cluster bounding box finding service
    rospy.loginfo('waiting for find_cluster_bounding_box service')
    rospy.wait_for_service('/find_cluster_bounding_box')
    find_bounding_box_srv = rospy.ServiceProxy('/find_cluster_bounding_box', FindClusterBoundingBox)
    rospy.loginfo('connected to find_cluster_bounding_box')
    
    target_location = msg.target_location
    t_p1 = msg.object_bbox.points[0]
    t_p2 = msg.object_bbox.points[1]
    
    t_p1.y, t_p2.y = t_p2.y, t_p1.y
    
    reset = ResetRobot()
    reset.execute()
    rospy.loginfo('Robot reset')
    
    segment = SegmentScene()
    res = segment.execute()
    if not res: exit(1)
    rospy.loginfo('Scene segmented')
    
    # find a cluster that intersects with a provided bounding box
    for idx,cluster in enumerate(res[1]['segmentation_result'].clusters):
        bbox_response = find_bounding_box_srv(cluster)
        
        if bbox_response.error_code != FindClusterBoundingBoxResponse.SUCCESS:
            rospy.logwarn('unable to find for cluster %d bounding box' % idx)
            continue
            
        o_center = bbox_response.pose.pose.position
        o_p1 = Point32(o_center.x - bbox_response.box_dims.x, o_center.y - bbox_response.box_dims.y, o_center.z)
        o_p2 = Point32(o_center.x + bbox_response.box_dims.x, o_center.y + bbox_response.box_dims.y, o_center.z)
        
        print idx, t_p1, t_p2, o_p1, o_p2
        
        if ((t_p1.x < o_p2.x and t_p1.y < o_p2.y) and
            (o_p1.x < t_p2.x and o_p1.y < t_p2.y)):
            closest_index = idx
            break
            
    grasp_client = SimpleActionClient('grasp_object', GraspObjectAction)
    grasp_client.wait_for_server()
    
    grasp_goal = GraspObjectGoal()
    grasp_goal.category_id = -1
    grasp_goal.graspable_object = res[0].graspable_objects[closest_index]
    grasp_goal.collision_object_name = res[0].collision_object_names[closest_index]
    grasp_goal.collision_support_surface_name = res[0].collision_support_surface_name
    
    result = grasp_client.send_goal_and_wait(grasp_goal)
    
    if result != GoalStatus.SUCCEEDED:
        rospy.logerr('failed to grasp')
        reset.execute()
        exit(1)
        
    rospy.loginfo('Grasped an object')
    lift_client = SimpleActionClient('lift_object', LiftObjectAction)
    lift_client.wait_for_server()
    
    lift_goal = LiftObjectGoal()
    lift_goal.category_id = 0
    lift_goal.graspable_object = res[0].graspable_objects[closest_index]
    lift_goal.collision_object_name = res[0].collision_object_names[closest_index]
    lift_goal.collision_support_surface_name = res[0].collision_support_surface_name
    
    result = lift_client.send_goal_and_wait(lift_goal)
    
    if result != GoalStatus.SUCCEEDED:
        rospy.logerr('failed to lift')
        exit(1)
        
    rospy.loginfo('Lifted an object')
    pudown_client = SimpleActionClient('put_down_at', PutDownAtAction)
    pudown_client.wait_for_server()
    
    putdown_goal = PutDownAtGoal()
    putdown_goal.category_id = -1
    putdown_goal.graspable_object = res[0].graspable_objects[closest_index]
    putdown_goal.collision_object_name = res[0].collision_object_names[closest_index]
    putdown_goal.collision_support_surface_name = res[0].collision_support_surface_name
    putdown_goal.target_location = Point(target_location.x, target_location.y, target_location.z) #Point(0.468, 0.226, 0.251)
    
    result = pudown_client.send_goal_and_wait(putdown_goal)
    
    if result != GoalStatus.SUCCEEDED:
        rospy.logerr('failed to put down')
        reset.execute()
        exit(1)
        
    rospy.loginfo('Pu down the object')
    reset.execute()
Пример #3
0
if __name__ == '__main__':
    rospy.init_node('grab_and_run')
    
    reset = ResetRobot()
    reset.execute()
    
    segment = SegmentScene()
    res = segment.execute()
    if not res: exit(1)
    
    closest_index = res[1][0][0]
    grasp_client = SimpleActionClient('grasp_object', GraspObjectAction)
    grasp_client.wait_for_server()
    
    grasp_goal = GraspObjectGoal()
    grasp_goal.category_id = -1
    grasp_goal.graspable_object = res[0].graspable_objects[closest_index]
    grasp_goal.collision_object_name = res[0].collision_object_names[closest_index]
    grasp_goal.collision_support_surface_name = res[0].collision_support_surface_name
    
    result = grasp_client.send_goal_and_wait(grasp_goal)
    
    if result != GoalStatus.SUCCEEDED:
        rospy.logerr('failed to grasp')
        reset.execute()
        exit(1)
        
    lift_client = SimpleActionClient('lift_object', LiftObjectAction)
    lift_client.wait_for_server()
    
Пример #4
0
if __name__ == '__main__':
    rospy.init_node('grab_and_run')

    reset = ResetRobot()
    reset.execute()

    segment = SegmentScene()
    res = segment.execute()
    if not res: exit(1)

    closest_index = res[1][0][0]
    grasp_client = SimpleActionClient('grasp_object', GraspObjectAction)
    grasp_client.wait_for_server()

    grasp_goal = GraspObjectGoal()
    grasp_goal.category_id = -1
    grasp_goal.graspable_object = res[0].graspable_objects[closest_index]
    grasp_goal.collision_object_name = res[0].collision_object_names[
        closest_index]
    grasp_goal.collision_support_surface_name = res[
        0].collision_support_surface_name

    result = grasp_client.send_goal_and_wait(grasp_goal)

    if result != GoalStatus.SUCCEEDED:
        rospy.logerr('failed to grasp')
        reset.execute()
        exit(1)

    lift_client = SimpleActionClient('lift_object', LiftObjectAction)
Пример #5
0
def do_it(msg):
    rospy.loginfo('We are DOING IT!')

    # connect to cluster bounding box finding service
    rospy.loginfo('waiting for find_cluster_bounding_box service')
    rospy.wait_for_service('/find_cluster_bounding_box')
    find_bounding_box_srv = rospy.ServiceProxy('/find_cluster_bounding_box',
                                               FindClusterBoundingBox)
    rospy.loginfo('connected to find_cluster_bounding_box')

    target_location = msg.target_location
    t_p1 = msg.object_bbox.points[0]
    t_p2 = msg.object_bbox.points[1]

    t_p1.y, t_p2.y = t_p2.y, t_p1.y

    reset = ResetRobot()
    reset.execute()
    rospy.loginfo('Robot reset')

    segment = SegmentScene()
    res = segment.execute()
    if not res: exit(1)
    rospy.loginfo('Scene segmented')

    # find a cluster that intersects with a provided bounding box
    for idx, cluster in enumerate(res[1]['segmentation_result'].clusters):
        bbox_response = find_bounding_box_srv(cluster)

        if bbox_response.error_code != FindClusterBoundingBoxResponse.SUCCESS:
            rospy.logwarn('unable to find for cluster %d bounding box' % idx)
            continue

        o_center = bbox_response.pose.pose.position
        o_p1 = Point32(o_center.x - bbox_response.box_dims.x,
                       o_center.y - bbox_response.box_dims.y, o_center.z)
        o_p2 = Point32(o_center.x + bbox_response.box_dims.x,
                       o_center.y + bbox_response.box_dims.y, o_center.z)

        print idx, t_p1, t_p2, o_p1, o_p2

        if ((t_p1.x < o_p2.x and t_p1.y < o_p2.y)
                and (o_p1.x < t_p2.x and o_p1.y < t_p2.y)):
            closest_index = idx
            break

    grasp_client = SimpleActionClient('grasp_object', GraspObjectAction)
    grasp_client.wait_for_server()

    grasp_goal = GraspObjectGoal()
    grasp_goal.category_id = -1
    grasp_goal.graspable_object = res[0].graspable_objects[closest_index]
    grasp_goal.collision_object_name = res[0].collision_object_names[
        closest_index]
    grasp_goal.collision_support_surface_name = res[
        0].collision_support_surface_name

    result = grasp_client.send_goal_and_wait(grasp_goal)

    if result != GoalStatus.SUCCEEDED:
        rospy.logerr('failed to grasp')
        reset.execute()
        exit(1)

    rospy.loginfo('Grasped an object')
    lift_client = SimpleActionClient('lift_object', LiftObjectAction)
    lift_client.wait_for_server()

    lift_goal = LiftObjectGoal()
    lift_goal.category_id = 0
    lift_goal.graspable_object = res[0].graspable_objects[closest_index]
    lift_goal.collision_object_name = res[0].collision_object_names[
        closest_index]
    lift_goal.collision_support_surface_name = res[
        0].collision_support_surface_name

    result = lift_client.send_goal_and_wait(lift_goal)

    if result != GoalStatus.SUCCEEDED:
        rospy.logerr('failed to lift')
        exit(1)

    rospy.loginfo('Lifted an object')
    pudown_client = SimpleActionClient('put_down_at', PutDownAtAction)
    pudown_client.wait_for_server()

    putdown_goal = PutDownAtGoal()
    putdown_goal.category_id = -1
    putdown_goal.graspable_object = res[0].graspable_objects[closest_index]
    putdown_goal.collision_object_name = res[0].collision_object_names[
        closest_index]
    putdown_goal.collision_support_surface_name = res[
        0].collision_support_surface_name
    putdown_goal.target_location = Point(
        target_location.x, target_location.y,
        target_location.z)  #Point(0.468, 0.226, 0.251)

    result = pudown_client.send_goal_and_wait(putdown_goal)

    if result != GoalStatus.SUCCEEDED:
        rospy.logerr('failed to put down')
        reset.execute()
        exit(1)

    rospy.loginfo('Pu down the object')
    reset.execute()
Пример #6
0
    def __init__(self):
        self.object_detector = ObjectDetector()

        # connect to collision map processing service
        rospy.loginfo('waiting for tabletop_collision_map_processing service')
        rospy.wait_for_service(
            '/tabletop_collision_map_processing/tabletop_collision_map_processing'
        )
        self.collision_map_processing_srv = rospy.ServiceProxy(
            '/tabletop_collision_map_processing/tabletop_collision_map_processing',
            TabletopCollisionMapProcessing)
        rospy.loginfo('connected to tabletop_collision_map_processing service')

        # connect to gripper action server
        rospy.loginfo('waiting for wubble_gripper_grasp_action')
        self.posture_controller = SimpleActionClient(
            '/wubble_gripper_grasp_action', GraspHandPostureExecutionAction)
        self.posture_controller.wait_for_server()
        rospy.loginfo('connected to wubble_gripper_grasp_action')

        rospy.loginfo('waiting for wubble_grasp_status service')
        rospy.wait_for_service('/wubble_grasp_status')
        self.get_grasp_status_srv = rospy.ServiceProxy('/wubble_grasp_status',
                                                       GraspStatus)
        rospy.loginfo('connected to wubble_grasp_status service')

        rospy.loginfo('waiting for classify service')
        rospy.wait_for_service('/classify')
        self.classification_srv = rospy.ServiceProxy('/classify', classify)
        rospy.loginfo('connected to classify service')

        # connect to gripper action server
        rospy.loginfo('waiting for wubble_gripper_action')
        self.gripper_controller = SimpleActionClient('/wubble_gripper_action',
                                                     WubbleGripperAction)
        self.gripper_controller.wait_for_server()
        rospy.loginfo('connected to wubble_gripper_action')

        # connect to wubble actions
        rospy.loginfo('waiting for drop_object')
        self.drop_object_client = SimpleActionClient('/drop_object',
                                                     DropObjectAction)
        self.drop_object_client.wait_for_server()
        rospy.loginfo('connected to drop_object')

        rospy.loginfo('waiting for grasp_object')
        self.grasp_object_client = SimpleActionClient('/grasp_object',
                                                      GraspObjectAction)
        self.grasp_object_client.wait_for_server()
        rospy.loginfo('connected to grasp_object')

        rospy.loginfo('waiting for lift_object')
        self.lift_object_client = SimpleActionClient('/lift_object',
                                                     LiftObjectAction)
        self.lift_object_client.wait_for_server()
        rospy.loginfo('connected to lift_object')

        rospy.loginfo('waiting for place_object')
        self.place_object_client = SimpleActionClient('/place_object',
                                                      PlaceObjectAction)
        self.place_object_client.wait_for_server()
        rospy.loginfo('connected to plcae_object')

        rospy.loginfo('waiting for push_object')
        self.push_object_client = SimpleActionClient('/push_object',
                                                     PushObjectAction)
        self.push_object_client.wait_for_server()
        rospy.loginfo('connected to push_object')

        rospy.loginfo('waiting for shake_roll_object')
        self.shake_roll_object_client = SimpleActionClient(
            '/shake_roll_object', ShakeRollObjectAction)
        self.shake_roll_object_client.wait_for_server()
        rospy.loginfo('connected to drop_object')

        rospy.loginfo('waiting for shake_pitch_object')
        self.shake_pitch_object_client = SimpleActionClient(
            '/shake_pitch_object', ShakePitchObjectAction)
        self.shake_pitch_object_client.wait_for_server()
        rospy.loginfo('connected to pitch_object')

        rospy.loginfo('waiting for ready_arm')
        self.ready_arm_client = SimpleActionClient('/ready_arm',
                                                   ReadyArmAction)
        self.ready_arm_client.wait_for_server()
        rospy.loginfo('connected to ready_arm')

        # advertise InfoMax service
        rospy.Service('get_category_distribution', InfoMax,
                      self.process_infomax_request)

        rospy.loginfo(
            'all services contacted, object_categorization is ready to go')

        self.ACTION_INFO = {
            InfomaxAction.GRASP: {
                'client': self.grasp_object_client,
                'goal': GraspObjectGoal(),
                'prereqs': [InfomaxAction.GRASP]
            },
            InfomaxAction.LIFT: {
                'client': self.lift_object_client,
                'goal': LiftObjectGoal(),
                'prereqs': [InfomaxAction.GRASP, InfomaxAction.LIFT]
            },
            InfomaxAction.SHAKE_ROLL: {
                'client':
                self.shake_roll_object_client,
                'goal':
                ShakeRollObjectGoal(),
                'prereqs': [
                    InfomaxAction.GRASP, InfomaxAction.LIFT,
                    InfomaxAction.SHAKE_ROLL
                ]
            },
            InfomaxAction.DROP: {
                'client':
                self.drop_object_client,
                'goal':
                DropObjectGoal(),
                'prereqs':
                [InfomaxAction.GRASP, InfomaxAction.LIFT, InfomaxAction.DROP]
            },
            InfomaxAction.PLACE: {
                'client':
                self.place_object_client,
                'goal':
                PlaceObjectGoal(),
                'prereqs':
                [InfomaxAction.GRASP, InfomaxAction.LIFT, InfomaxAction.PLACE]
            },
            InfomaxAction.PUSH: {
                'client':
                self.push_object_client,
                'goal':
                PushObjectGoal(),
                'prereqs': [
                    InfomaxAction.GRASP, InfomaxAction.LIFT,
                    InfomaxAction.PLACE, InfomaxAction.PUSH
                ]
            },
            InfomaxAction.SHAKE_PITCH: {
                'client':
                self.shake_pitch_object_client,
                'goal':
                ShakePitchObjectGoal(),
                'prereqs': [
                    InfomaxAction.GRASP, InfomaxAction.LIFT,
                    InfomaxAction.SHAKE_PITCH
                ]
            },
        }