Пример #1
0
def main(nh):
    sub = yield sub_scripting.get_sub(nh)
    
    sub.move.go(linear=[0.25, 0, 0])
    
    yield sub.visual_approach('forward', 'shooter', size_estimate=7*.0254, desired_distance=1.5, selector=select_by_body_direction([0,1,0]))
    yield util.sleep(5)
    yield sub.move.forward(.5).go()
    
    yield sub.move.up(5*.0254).go()
    yield sub.move.right(3.5*.0254).go()

    yield util.sleep(5)
    yield sub.move.backward(2).go()

    yield sub.fire_left_torpedo()

    yield sub.visual_approach('forward', 'shooter', size_estimate=7*.0254, desired_distance=1.5, selector=select_by_body_direction([0,-1,0]))
    yield util.sleep(5)
    yield sub.move.forward(.5).go()
    
    yield sub.move.up(5*.0254).go()
    yield sub.move.left(3.5*.0254).go()
    yield util.sleep(5)
    yield sub.move.backward(2).go()
    yield sub.fire_right_torpedo()
    """yield sub.move.right(.6).go()
Пример #2
0
def main(nh):
    sub = yield sub_scripting.get_sub(nh)
    
    for i in xrange(4):
        print 'Side', i
        yield sub.move.forward(2).go()
        yield sub.move.turn_left_deg(90).go()
Пример #3
0
def main(nh):
    print "starting shooter"
    sub = yield sub_scripting.get_sub(nh)
    
    fwd_move = sub.move.go(linear=[0.25, 0, 0])
    
    try:
        obj = yield util.wrap_timeout(sub.visual_approach('forward', 'shooter/hole', size_estimate=5*.0254, desired_distance=1.5, selector=select_by_body_direction([0,1,0])), 30)
    finally:
        yield fwd_move.cancel()
    
    start = sub.move
    #try:
    #    yield util.wrap_timeout(align(sub), 30)
    #except Exception:
    #    yield start.go() # go back because alignment failed
    
    yield sub.visual_approach('forward', 'shooter/hole', size_estimate=5*.0254, desired_distance=1.0, selector=select_by_body_direction([0,1,0])) 
    yield util.sleep(5)
    yield sub.move.forward(.3).go()
    yield sub.move.up(5*.0254).go()
    yield sub.move.right(3.5*.0254).go()
    yield sub.fire_left_torpedo()
    yield sub.move.backward(2.0).go()

    print 'going to second hole'
    yield sub.visual_approach('forward', 'shooter/hole', size_estimate=5*.0254, desired_distance=1.0, selector=select_by_body_direction([0,-1,0]))
    yield util.sleep(5)
    yield sub.move.forward(0.3).go()

    yield sub.move.up(5*.0254).go()
    yield sub.move.left(3.5*.0254).go()
    yield sub.fire_right_torpedo()
    yield sub.move.backward(1.0).go()
    """yield sub.move.right(.6).go()
Пример #4
0
def main(nh):
    sub = yield sub_scripting.get_sub(nh)

    begin = False 

    while True:
        begin = yield sub.get_begin()
        if begin == True:
            break

    print "Starting mission"

    '''

    Vision order:
        recovery_vision
        torpedo_vision
        torpedo_area_vision
        train_vision
        path_vision
        buoys_vision

    '''

    #sub.change_current_vision(False,False,False,False,False,False)

    # WRAP ENTIRE MISSION IN TIMEOUT
    try:
        yield util.wrap_timeout(main_list(nh, sub), TOTAL_TIME)
    except Exception:
        traceback.print_exc()
Пример #5
0
def main_list(nh):
    sub = yield sub_scripting.get_sub(nh)
    yield sub.move.depth(1).go()
    yield sub.move.forward(15).go()
    m = numpy.array([7.6448368633, 41.202367025499996, 0]) * .8
    yield sub.move.look_at_rel_without_pitching(m).go()
    yield sub.move.absolute(m).go()
Пример #6
0
def main(nh, target = None, orient_target = None):
    sub = yield sub_scripting.get_sub(nh)
    picked_up_target = False

    align_target = target
    if align_target == None: align_target = 'delorean'
    orientation_align_target = orient_target
    if orientation_align_target == None: orientation_align_target = 'delorean'

    x_location = 0
    y_location = 0

    while not rospy.is_shutdown():

        yield sub.orient_and_align(align_target, orientation_align_target, MOVE_SCALE, ANGULAR_TOLERANCE, ORIENTATION_TOLERANCE)
    # Move down until distance from obbject is 1 meter
    yield move_down(sub)

    while picked_up_target == False:
        yield sub.close_gripper()
        yield sub.move.up(1).go()
        # Scan ten times to verify that we have picked up object
        for x in xrange(1,10):
            target_scan = yield sub.get_target_location(align_target)
            yield util.sleep(.1)
            if target_scan.x == 0 and target_scan.y == 0: picked_up_target = True
Пример #7
0
def main(nh):
    sub = yield sub_scripting.get_sub(nh)

    for i in xrange(4):
        print 'Side', i
        yield sub.move.forward(2).go()
        yield sub.move.turn_left_deg(90).go()
Пример #8
0
def main(nh):
    sub = yield sub_scripting.get_sub(nh)
    
    #yield sub.lower_down_grabber()
    while True:
        yield util.sleep(2)
        yield sub.lower_down_grabber()
        yield sub.open_down_grabber()
        yield util.sleep(2)
Пример #9
0
def main(nh):
    sub = yield sub_scripting.get_sub(nh)
    
    orig_depth = -sub.pose.position[2]
    
    yield sub.visual_approach_3d('forward', 1.5, buoy_targetdesc)
    
    yield sub.move.forward(1.5).go()
    yield sub.move.forward(-6).go()
Пример #10
0
def main(nh):
    sub = yield sub_scripting.get_sub(nh)

    yield sub.visual_approach_3d('forward', 1.5)

    yield sub.move.forward(1.5).go()
    yield sub.move.backward(1.5).go()

    yield sub.move.depth(0.5).go()
Пример #11
0
def main(nh):
    sub = yield sub_scripting.get_sub(nh)

    print "firing left torpedo"
    yield sub.fire_left_torpedo()
    print "fired, waiting 3 seconds"
    yield util.sleep(3)
    print "firing right torpedo"
    yield sub.fire_right_torpedo()
    print "fired right torpedo"
Пример #12
0
def main(nh):
    sub = yield sub_scripting.get_sub(nh)

    print "lowering impaler"
    yield sub.lower_impaler()
    print "lowered, waiting 3 seconds"
    yield util.sleep(3)
    print "raising impaler"
    yield sub.raise_impaler()
    print "raised impaler"
Пример #13
0
def main(nh):
    sub = yield sub_scripting.get_sub(nh)
    
    sub.move.go(linear=[0.25, 0, 0])
    
    yield sub.visual_approach('forward', 'grapes/board', size_estimate=math.sqrt(2)*3*12*.0254, desired_distance=2)
    
    goal_mgr = sub._camera_2d_action_clients['forward'].send_goal(legacy_vision_msg.FindGoal(
        object_names=['grapes/empty_cell'],
    ))
    feedback = yield goal_mgr.get_feedback()
    res = map(json.loads, feedback.targetreses[0].object_results)
    
    res.sort(key=lambda x: float(x['redness']))
    
    all_possible_coords=set((X,Y) for X in [-1, 0, 1] for Y in [-1, 0, 1] if X != 0 or Y != 0)
    coords = max(itertools.permutations(all_possible_coords, len(res)),
        key=lambda positions: min(
            pearsonr(*zip(*((pos[0], float(obj['center'][0])) for pos, obj in zip(positions, res)))),
            pearsonr(*zip(*((pos[1], float(obj['center'][1])) for pos, obj in zip(positions, res)))),
        )
    )
        
    empty = res[:4]
    filled = res[4:8]
    
    for x in res:
        print x['redness'], x['center']

    
    '''empty_coords = min(itertools.permutations(all_possible_coords, 4),
        key=lambda positions: sum(math.sqrt(
            (float(obj['center'][0]) - .12*pos[0])**2 +
            (float(obj['center'][1]) - .12*pos[1])**2
        ) for pos, obj in zip(positions, empty)),
    )'''
    empty_coords = coords[:4]
    print empty_coords
    
    empty_coords = set(empty_coords)
    
    filled_coords = all_possible_coords - empty_coords
    print empty_coords, filled_coords
    
    def gen_paths(unmoved_peg_coords, empty_coords):
        if not unmoved_peg_coords:
            yield []
            return
        for a in unmoved_peg_coords:
            for b in empty_coords:
                for rest in gen_paths(unmoved_peg_coords - {a}, (empty_coords | {a}) - {b}):
                    yield [(a, b)] + rest

    def dist((ax, ay), (bx, by)):
        return math.sqrt((ax-bx)**2 + (ay-by)**2)
Пример #14
0
def main(nh):
    sub = yield sub_scripting.get_sub(nh)

    sub.move.go(linear=[0.25, 0, 0])

    yield sub.visual_align('down', 'wreath')

    orig_depth = -sub._pose.position[2]  # XXX don't use private attr
    yield sub.move.down(1).go()
    yield sub.move.depth(-0.1).go()
    yield sub.move.depth(orig_depth).go()
Пример #15
0
def main(nh):
    sub = yield sub_scripting.get_sub(nh)
    
    sub.move.go(linear=[0.25, 0, 0])
    
    yield sub.visual_align('down', 'wreath')
    
    orig_depth = -sub._pose.position[2] # XXX don't use private attr
    yield sub.move.down(1).go()
    yield sub.move.depth(-0.1).go()
    yield sub.move.depth(orig_depth).go()
Пример #16
0
def main(nh):
    sub = yield sub_scripting.get_sub(nh)

    dist = yield sub.get_dvl_range()

    print dist

    sub.move.go(linear=[0.25, 0, 0])

    yield sub.visual_align('down', 'pipe', dist)

    yield sub.move.forward(2).go()
Пример #17
0
def main(nh):
    sub = yield sub_scripting.get_sub(nh)
    
    dist = yield sub.get_dvl_range()
    
    print dist
    
    sub.move.go(linear=[0.25, 0, 0])
    
    yield sub.visual_align('down', 'pipe', dist)
    
    yield sub.move.forward(2).go()
Пример #18
0
def main(nh):
    sub = yield sub_scripting.get_sub(nh)
    
    sub.move.go(linear=[0.25, 0, 0])
    
    yield sub.visual_approach('forward', 'hedge', size_estimate=4*12*.0254, desired_distance=2)
    
    yield sub.move.right(.6).go()
    yield sub.move.forward(3.5).go()
    yield sub.move.left(1.2).go()
    yield sub.move.backward(3.5).go()
    yield sub.move.right(1.2).go()
    yield sub.move.forward(5).go()
Пример #19
0
def main(nh):
    sub = yield sub_scripting.get_sub(nh)
    
    sub.move.go(linear=[0.25, 0, 0])

    dist = yield sub.get_dvl_range()

    
    yield sub.visual_align('down', 'wreath', dist)
    
    orig_depth = -sub.pose.position[2]
    yield sub.move.down(1).go()
    yield sub.move.depth(-0.1).go()
    yield sub.move.depth(orig_depth).go()
Пример #20
0
def main(nh):

    sub = yield sub_scripting.get_sub(nh)

    global door_pos
    global centered_pos

    # Align sub to center of target
    yield sub.align('center', MOVE_SCALE, ANGULAR_TOLERANCE)
    # Move sub until it is one meter away from target
    yield move_to_target(sub)
    # Grab centered position of the sub
    centered_pos = yield sub.pose.position

    '''
Пример #21
0
def main(nh, sub=None):
    print "Q"
    if sub is None:
        sub = yield sub_scripting.get_sub(nh)

        #forward_move = numpy.linalg.norm((x,y)-(x,y))
    while True:
        
        angles = yield sub.get_green_buoy()
        print "Buoy at", angles
        if abs(angles.x)>10 or abs(angles.y)>10:
            if angles.x<0:
                yield sub.move.left(abs(angles.x/500)).go()
            if angles.x>0:
                yield sub.move.right(abs(angles.x/500)).go()
            if angles.y>0:
                yield sub.move.down(abs(angles.y/500)).go()
            if angles.y<0:
                yield sub.move.up(abs(angles.y/500)).go()
        else:
            #yield sub.move.forward(1)).go()
            break
    

    print 'CENTERED ON BUOY'
    #yield sub.move.forward(forward_move/2).go()
    yield sub.move.forward(3.5).go()

    while True:
        
        angles = yield sub.get_green_buoy()
        print "Buoy at", angles
        if abs(angles.x)>10 or abs(angles.y)>10:
            if angles.x<0:
                yield sub.move.left(abs(angles.x/500)).go()
            if angles.x>0:
                yield sub.move.right(abs(angles.x/500)).go()
            if angles.y>0:
                yield sub.move.down(abs(angles.y/500)).go()
            if angles.y<0:
                yield sub.move.up(abs(angles.y/500)).go()
        else:
            #yield sub.move.forward(1)).go()
            break
            
    print 'CENTERED ON BUOY'
    #yield sub.move.forward(forward_move/2).go()
    yield sub.move.forward(1).go()
Пример #22
0
def main(nh, direction=None):
    sub = yield sub_scripting.get_sub(nh)
    
    dist = yield sub.get_dvl_range()
    
    print dist
    
    sub.move.go(linear=[0.25, 0, 0])
    
    yield sub.visual_align('down', 'pipe', dist, selector={
        'left' : select_by_body_direction([0, +1, 0]),
        'right': select_by_body_direction([0, -1, 0]),
        None   : lambda objs: objs[0],
    }[direction])
    
    yield sub.move.forward(2).go()
Пример #23
0
def main(nh):
    #while True:
    #    time_left_str = yield util.nonblocking_raw_input('Enter time left: (e.g. 5:40) ')
    #    try:
    #        m, s = time_left_str.split(':')
    #        time_left = 60 * int(m) + int(s)
    #    except Exception:
    #        traceback.print_exc()
    #    else:
    #        break
    
    sub = yield sub_scripting.get_sub(nh)
    
    try:
        yield util.wrap_timeout(main_list(nh), 11*60)
    except Exception:
        traceback.print_exc()
    
    yield fail_list(nh)
Пример #24
0
def main(nh, direction=None, orient_away_from=False, forward=True, depth=0.75):
    sub = yield sub_scripting.get_sub(nh)
    
    yield sub.move.depth(depth).go()
    
    dist = yield sub.get_dvl_range()
    
    print dist
    
    start_pos = sub.pose.position
    if forward:
        fwd_move = sub.move.go(linear=[0.25, 0, 0])
    try:
        yield sub.visual_align('down', 'pipe', dist, selector={
            'left' : sub_scripting.select_by_body_direction([0, +1, 0]),
            'right': sub_scripting.select_by_body_direction([0, -1, 0]),
            None   : lambda items, body_tf: items[0],
        }[direction], orient_away_from=start_pos if orient_away_from else None)
    finally:
        if forward:
            yield fwd_move.cancel()
Пример #25
0
def main(nh, freq=25e3):
    sub = yield sub_scripting.get_sub(nh)
    yield sub.raise_down_grabber()

    if 1:
        yield sub.move.depth(1).go()
        fwd_move = sub.move.go(linear=[.3, 0, 0])
        try:
            yield sub.hydrophone_align(freq)
        finally:
            fwd_move.cancel()
        yield sub.move.relative(RELATIVE_PINGER_MOVE).go()
        
        #print 'surfacing'
        #yield sub.move.depth(0).go()
        
        yield sub.move.depth(0.4).go()
        bin_pose = sub.move
        try:
            yield util.wrap_timeout(sub.visual_align('down', 'wreath/bin/high', 2, selector=select_centered, turn=False), 10)
            yield sub.move.relative(RELATIVE_VISION_MOVE).go()
        except util.TimeoutError:
            print 'bin alignment timed out'
            yield bin_pose.go()
        except:
            print 'bin alignment???'
            yield bin_pose.go()
        yield path.main(nh, orient_away_from=True, forward=False, depth=0.4)
        yield sub.move.forward(2).go()
    
    yield retry_to_grab(sub, 'moonrock', freq, surface=True)
    yield retry_to_grab(sub, 'moonrock', freq, bubbles=True, surface=True)
    yield retry_to_grab(sub,   'cheese', freq)
    yield retry_to_grab(sub,   'cheese', freq)
    yield retry_to_grab(sub,   'cheese', freq, bubbles=True)
    yield retry_to_grab(sub, 'moonrock', freq)
    while True:
        yield retry_to_grab(sub, 'moonrock', freq)
Пример #26
0
def main(nh):
    sub = yield sub_scripting.get_sub(nh)
    
    orig_depth = -sub.pose.position[2]

    #yield sub.move.forward(1).go()
    print "aligning down"
    dist = yield sub.get_dvl_range()
    fwd_move = sub.move.go(linear=[0.25, 0, 0])
    try:
        yield sub.visual_align('down', 'bins/all', distance_estimate=dist-.3, turn=True, angle=math.radians(45))
    finally:
        fwd_move.cancel()
    dist = yield sub.get_dvl_range()
    yield sub.visual_align('down', 'bins/all', distance_estimate=dist-.3, turn=False)
    #yield sub.move.down(dist-.3 - 2.5).go()
    print "aligned down"
    yield sub.move.down(dist - 3.2).go()
    dist = yield sub.get_dvl_range()
    centered = sub.move
    
    for x in ["1", "4"]:
        print 'going to', x
        yield centered.go()
        print "aligning single", x
        dist = yield sub.get_dvl_range()
        yield sub.visual_align('down', 'bins/single', distance_estimate=dist-.3, selector=select(x), turn=False)
        print 'done'
        yield sub.move.down(dist - 2).go()
        yield sub.visual_align('down', 'bins/single', distance_estimate=2, selector=select_centered, turn=True)
        yield sub.move.backward(.25).go()
        print 'dropping'
        yield sub.drop_ball()
        yield util.sleep(3)
        print 'done dropping'
    
    yield sub.move.depth(orig_depth).go()
Пример #27
0
def main(nh, direction=None):
    sub = yield sub_scripting.get_sub(nh)
    
    yield sub.move.depth(0.75).go()
    
    yield sub.hydrophone_align(25e3)
Пример #28
0
def main(nh):
    sub = yield sub_scripting.get_sub(nh)
    
    yield sub.fire_left_torpedo()
Пример #29
0
def fail_list(nh):
    sub = yield sub_scripting.get_sub(nh)
    yield recovery.main(nh)
Пример #30
0
def main(nh):
    sub = yield sub_scripting.get_sub(nh)
    
    fwd_move = sub.move.go(linear=[0.25, 0, 0])
    try:
        yield sub.visual_approach('forward', 'grapes/board', size_estimate=math.sqrt(2)*3*12*.0254, desired_distance=2)
    finally:
        yield fwd_move.cancel()
    """
    goal_mgr = sub._camera_2d_action_clients['forward'].send_goal(legacy_vision_msg.FindGoal(
        object_names=['grapes/board2'],
    ))
    feedback = yield goal_mgr.get_feedback()
    res = map(json.loads, feedback.targetreses[0].object_results)  
    
    print 'about to align'  
    while True:
        print 'aligning'
        feedback = yield goal_mgr.get_feedback()
        res = map(json.loads, feedback.targetreses[0].object_results)
        if not res:
            continue
        #goal_mgr.cancel()
        angle = float(res[0]['orientation_error'])
        xdist = 1.5 * math.sin(angle/2)
        print angle, xdist
        yield sub.move.yaw_left(angle).go()
        yield sub.move.right(2*xdist).go()

        if abs(angle)<math.radians(5):
                break

    print 'done aligning'
    """
    goal_mgr = sub._camera_2d_action_clients['forward'].send_goal(legacy_vision_msg.FindGoal(
        object_names=['grapes/empty_cell'],
    ))
    feedback = yield goal_mgr.get_feedback()
    res = map(json.loads, feedback.targetreses[0].object_results)
    
    res.sort(key=lambda x: float(x['redness']))
    
    all_possible_coords=set((X,Y) for X in [-1, 0, 1] for Y in [-1, 0, 1] if X != 0 or Y != 0)
    coords = max(itertools.permutations(all_possible_coords, len(res)),
        key=lambda positions: min(
            pearsonr(*zip(*((pos[0], float(obj['center'][0])) for pos, obj in zip(positions, res)))),
            pearsonr(*zip(*((pos[1], float(obj['center'][1])) for pos, obj in zip(positions, res)))),
        )
    )
        
    empty = res[:4]
    filled = res[4:8]
    
    for x in res:
        print x['redness'], x['center']

    
    '''empty_coords = min(itertools.permutations(all_possible_coords, 4),
        key=lambda positions: sum(math.sqrt(
            (float(obj['center'][0]) - .12*pos[0])**2 +
            (float(obj['center'][1]) - .12*pos[1])**2
        ) for pos, obj in zip(positions, empty)),
    )'''
    empty_coords = coords[:4]
    print empty_coords
    
    empty_coords = set(empty_coords)
    
    filled_coords = all_possible_coords - empty_coords
    print empty_coords, filled_coords
    
    def gen_paths(unmoved_peg_coords, empty_coords):
        if not unmoved_peg_coords:
            yield []
            return
        for a in unmoved_peg_coords:
            for b in empty_coords:
                for rest in gen_paths(unmoved_peg_coords - {a}, empty_coords - {b}):
                    yield [(a, b)] + rest

    def dist((ax, ay), (bx, by)):
        return math.sqrt((ax-bx)**2 + (ay-by)**2)
Пример #31
0
def main(nh):
    sub = yield sub_scripting.get_sub(nh)

    pose = yield sub.pose.position
    print pose
Пример #32
0
def main(nh):
    sub = yield sub_scripting.get_sub(nh)
    buoy_targetdesc.prior_distribution.pose.orientation = Quaternion(*sub.pose.orientation)
    
    yield sub.visual_approach_3d('forward', 1.5, buoy_targetdesc)
Пример #33
0
def main(nh):
    sub = yield sub_scripting.get_sub(nh)

    print "dropping ball"
    yield sub.drop_ball()
    print "ball dropped"