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()
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()
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()
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()
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()
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
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()
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)
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()
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()
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"
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"
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)
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()
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()
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()
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()
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()
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()
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
'''
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()
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()
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)
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()
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)
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()
def main(nh, direction=None):
sub = yield sub_scripting.get_sub(nh)
yield sub.move.depth(0.75).go()
yield sub.hydrophone_align(25e3)
def main(nh):
sub = yield sub_scripting.get_sub(nh)
yield sub.fire_left_torpedo()
def fail_list(nh):
sub = yield sub_scripting.get_sub(nh)
yield recovery.main(nh)
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)
def main(nh):
sub = yield sub_scripting.get_sub(nh)
pose = yield sub.pose.position
print pose
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)
def main(nh):
sub = yield sub_scripting.get_sub(nh)
print "dropping ball"
yield sub.drop_ball()
print "ball dropped"
