def on_first_run(self, vision, *args, **kwargs):
table = shm.recovery_world_table.get()
def record_position():
table.know_pos = True
table.north = shm.kalman.north.get()
table.east = shm.kalman.east.get()
shm.recovery_world_table.set(table)
self.use_task(
Sequential(
# Move to the table depth first so we don't blow things off
Log('Moving to table depth'),
FastDepth(constants.table_depth),
Sequential(
Log('Returning to table position ({}, {})'.format(
table.north, table.east)),
GoToPositionRough(table.north, table.east, optimize=False),
) if table.know_pos else NoOp(),
Log('Searching for table'),
MasterConcurrent(
IdentifyObjects(lambda: vision.ellipses, 2),
SearchWithGlobalTimeout(timeout=self.TIMEOUT)),
Log('Centering table'),
CenterCentroid(lambda: vision.coords(vision.ellipses)),
Zero(),
Log('Recording table position'),
FunctionTask(record_position),
))
def both_paths():
return Sequential(Log('Going to Search Depth'), Depth(PATH_SEARCH_DEPTH),
Zero(), Log('Beginning to align to Path 1'),
one_path(shm.path_results_1), Log('Moving forward'),
Timed(VelocityX(.2), 1),
Log('Beginning to align to Path 2'),
one_path(shm.path_results_2), Log('Done'), Zero())
def on_first_run(self, vision, tube, amlan):
self.use_task(
Sequential(
Timeout(
AlignAmlan(
vision,
lambda: vision.obj_with_id(vision.tubes, tube),
amlan,
Altitude(constants.tube_dive_altitude,
overshoot_protect=True,
deadband=0.07),
align_during_depth=True,
), self.ALIGN_TIMEOUT),
Log('Moving to tube grab altitude'),
Conditional(
Timeout(Altitude(constants.tube_grab_altitude),
self.GRAB_ALTITUDE_TIMEOUT),
on_fail=Log(
'Warning: grab altitude not fully reached, grabbing anyway'
),
),
Timer(1),
amlan.Extend(),
Timer(1.5),
Log('Plucking tube'),
Timeout(RelativeToInitialDepth(-0.25, error=0.10), 10),
))
def second_pipe(grp):
return Sequential(
Log('Going to Search Depth'),
Depth(PIPE_SEARCH_DEPTH),
Retry(lambda: Sequential(
Zero(),
Log('Sway searching for pipe'),
search_task(),
Log('Centering on pipe'),
center(),
Depth(PIPE_FOLLOW_DEPTH),
Log('Aligning with pipe'),
Concurrent(
align(),
center(),
finite=False
),
), float("inf")),
Zero(),
Log('Aligned, moving forward'),
Timed(VelocityX(.4),3),
Zero()
#Depth(PIPE_FOLLOW_DEPTH)
)
def on_first_run(self, is_bat_getter, wrapped_task):
is_bat = is_bat_getter()
get_x = shm.bins_status.bat_x.get if is_bat else shm.bins_status.wolf_x.get
get_y = shm.bins_status.bat_y.get if is_bat else shm.bins_status.wolf_y.get
pointing_wrong_way = -1 if is_bat ^ (get_x() < 0) else 1
x_amt = .8
y_amt = 1.5 if is_bat else -1.5
turn_amt = -90 if is_bat else 90
r_x_amt = x_amt * pointing_wrong_way
r_y_amt = y_amt * pointing_wrong_way
r_turn_amt = turn_amt * pointing_wrong_way
ret_y_amt = -.8 if is_bat else .8
self.use_task(
Sequential(
FakeMoveY(r_y_amt, .3),
Timer(.2),
FakeMoveX(r_x_amt, .3),
Timer(.2),
RelativeToInitialHeading(r_turn_amt),
wrapped_task,
Log("Moving back"),
# might have to deal with heaeding here
FakeMoveX(-.8, .3),
Log("moving Y {}".format(ret_y_amt)),
FakeMoveY(ret_y_amt, .3), # intentionally swapped
Timer(.5),
#Log("moving X {}".format(y_amt * math.sin(math.radians(turn_amt)))),
#FakeMoveX(y_amt * math.sin(math.radians(turn_amt)), .3), # intentionally swapped
FakeMoveX(1.5, .3),
))
def on_first_run(self, vision, amlan, blind=False):
initial_tube = amlan.shm_tube.get()
def apply_drop():
# Add tube to table
table_tubes = get_shm_array(shm.recovery_world_table, 'has_tube')
table_tubes[initial_tube] = True
set_shm_array(shm.recovery_world_table, 'has_tube', table_tubes)
self.use_task(
Sequential(
AlignAmlan(
vision,
lambda: vision.obj_with_id(vision.ellipses,
amlan.shm_tube.get()),
amlan,
FastDepth(constants.ellipse_depth),
blind=blind,
align_during_depth=True,
),
Sequential(
Log('Moving to tube drop depth'),
GradualDepth(constants.drop_depth),
) if not blind else NoOp(),
Timer(1),
amlan.Retract(),
FunctionTask(apply_drop),
Sequential(
Log('Moving up away from table slowly'),
GradualDepth(constants.drop_depth + self.AFTER_DROP_DELTA),
) if not blind else NoOp(),
))
def on_first_run(self, vision, *args, **kwargs):
def check_removed():
bin = vision.target_bin()
return bin is not None and not bin.obs.covered
def CheckRemoved():
return FunctionTask(check_removed)
self.use_task(Zeroed(Conditional(
Sequential(
MoveAboveBins(vision),
Conditional(
CheckRemoved(),
Log('The cover is already gone?!?'),
Sequential(
Zero(),
AlignOverTargetBin(vision, 90),
LiftOffCover(vision),
Log('Verifying the cover was removed'),
MoveAboveBins(vision),
CheckRemoved(),
),
),
),
on_fail=Fail(Log('Failed to remove cover')),
)))
def LogSuccess(task):
name = task.__class__.__name__
return Conditional(
task,
Log('{} succeeded!'.format(name)),
Fail(Log('{} failed!'.format(name))),
)
def on_first_run(self, *args, **kwargs):
self.use_task(
Sequential(
Log('Going to gate depth'),
Depth(constants.depth),
Log('Moving through gate'),
Timed(VelocityX(0.4), constants.vel_time),
Zero(),
))
def on_first_run(self, vision, *args, **kwargs):
self.use_task(Conditional(
Sequential(
Retry(lambda: MoveAboveBins(vision), 3),
RemoveCover(vision),
),
Log('Bin uncovered!'),
Fail(Log('Failed to uncover bin')),
))
def on_first_run(self, vision, *args, **kwargs):
self.use_task(Conditional(
Sequential(
Log('Starting drop'),
Retry(lambda: MoveAboveBins(vision), 3),
AlignOverTargetBin(vision, 90, double_align=True),
FireMarkers(),
),
on_fail=Fail(Log('Failed to drop')),
))
def on_first_run(self, vision, single_bin, *args, **kwargs):
self.use_task(Conditional(
Sequential(
MoveAboveBins(vision),
Timer(0.5), # Wait for vision to stabilize
FunctionTask(lambda: vision.classify(single_bin)),
),
Log('Bins classified'),
Fail(Log('Failed to classify bins')),
))
def on_first_run(self, *args, **kwargs):
original_depth = shm.kalman.depth.get()
self.use_task(
WithPositionalControl(
Sequential(
Log('Getting close to surface'),
FastDepth(0.5),
Log('Surfacing!'),
Timed(Depth(0), 3),
Log('Falling back below surface'),
FastDepth(original_depth),
)))
def on_first_run(self, vision, *args, **kwargs):
def AvoidWall():
if not shm.recovery_world_table.know_pos.get():
return Sequential(
Log('Moving away from wall'),
ConsistentTask(Heading(WALL_TOWER_HEADING)),
MoveXRough(2),
)
else:
return NoOp()
def Iteration(first_run=False):
return Sequential(
# Sequential(
# Log('Classifying tubes'),
# Retry(lambda: MoveAboveTower(vision), 3),
# FunctionTask(vision.classify_tubes),
# ) if first_run else NoOp(),
GetTube(vision),
Succeed(GetTube(
vision)), # It's OK if we don't grab the second tube
Surface(),
AvoidWall(),
# Initially classify ellipses as their size / color may change
# as tubes are added
Sequential(
Log('Classifying ellipses'),
Retry(lambda: MoveAboveTable(vision), 3),
FunctionTask(vision.classify_ellipses),
) if first_run else NoOp(),
PlaceTubes(vision),
)
self.use_task(
Except(
Sequential(
Retry(lambda: MoveAboveTable(vision), 1000),
Log('Moving towards wall'),
ConsistentTask(Heading((WALL_TOWER_HEADING + 180) % 360)),
MoveXRough(2),
Iteration(first_run=True),
While(
Iteration,
lambda: count_tubes(shm.recovery_world_tower) > 0,
),
),
Fail(Log('Global timeout, done trying recovery T_T')),
GlobalTimeoutError,
))
def on_first_run(self, vision, tube, amlan, *args, **kwargs):
def apply_grab():
# Add tube to Amlan in shm
amlan.shm_tube.set(tube)
# Remove tube from tower in shm
tower_tubes = get_shm_array(shm.recovery_world_tower, 'has_tube')
tower_tubes[tube] = False
set_shm_array(shm.recovery_world_tower, 'has_tube', tower_tubes)
# Remove tube from vision pattern
vision.remove_tube(tube)
north = shm.kalman.north.get()
east = shm.kalman.east.get()
depth = shm.desires.depth.get()
initial_tubes = sum(t.obs is not None for t in vision.tubes)
self.use_task(
Conditional(
Sequential(
amlan.FastRetract(),
Defer(
Conditional(
GrabTube(vision, tube, amlan),
on_fail=Fail(
Sequential(
Log('Failed to grab, prematurely retracting grabber'
),
amlan.FastRetract(),
)),
),
Sequential(
Zero(),
Log('Restoring position from before grab'),
FastDepth(depth),
GoToPositionRough(north, east, optimize=False),
),
),
# TODO continue always assuming good grab?
# VerifyGrab(vision, initial_tubes),
FunctionTask(apply_grab),
),
Log('Grabbed {} tube with {} Amlan!'.format(
constants.colors[tube].name,
amlan.name,
)),
Fail(amlan.FastRetract()),
))
def on_first_run(self, vision, *args, **kwargs):
self.use_task(Conditional(
Sequential(
Log('Moving to depth where bins are visible'),
Depth(constants.see_both_depth, error=0.2, *args, **kwargs),
Log('Searching for bin'),
MasterConcurrent(IdentifyBin(vision), SearchWithGlobalTimeout()),
Log('Centering bins'),
CenterBins(vision),
),
on_fail=Fail(Log('Failed to move above bins')),
))
def on_first_run(self,
vision,
obj_func,
amlan,
align_depth_task,
blind=False,
align_during_depth=False,
*args,
**kwargs):
def tube_angle_heading():
return shm.kalman.heading.get() + (obj_func().obs.angle % 180 - 90)
self.must_see_obj = not blind
def unsee():
self.must_see_obj = False
self.task = Sequential(
Sequential(
Log('Centering object'),
CenterCentroid(lambda: vision.coords([obj_func()]),
precision=0),
Zero(),
Sequential(
Log('Aligning to object'),
Concurrent(
CenterCentroid(lambda: vision.coords([obj_func()]),
precision=0),
GradualHeading(tube_angle_heading),
finite=False,
),
) if not align_during_depth else NoOp(),
Log('Going down and precisely aligning to object'),
ConsistentTask(
Concurrent(
GradualHeading(tube_angle_heading),
CenterCentroid(lambda: vision.coords([obj_func()]),
precision=2),
align_depth_task,
finite=False,
)),
FunctionTask(unsee),
) if not blind else NoOp(),
PositionalControl(),
Zero(),
Log('Applying Amlan offset'),
amlan.CenterFromDownCam(),
)
def on_first_run(self, *args, **kwargs):
self.use_task(Sequential(
Log('Firing markers!'),
MoveX(-self.MARKERS_X_OFFSET, deadband=0.02),
FireActuator('left_marker', self.FIRE_TIME),
FireActuator('right_marker', self.FIRE_TIME),
))
def Full(): return Sequential(
Log('Changing depth before hydrophones'),
Depth(settings.depth),
OptimizablePinger(),
)
full = Full()
def testLeftRestore():
return TrackMovementY(
Sequential(
Timed(VelocityY(-.4), 6),
Log('Restoring?'),
RestorePosY(.2),
Zero(),
))
def on_first_run(self, *args, **kwargs):
self.use_task(Sequential(
Log('Ending Run! Surfacing and softkilling'),
Zero(),
Depth(0),
FunctionTask(lambda: shm.switches.soft_kill.set(1)),
FunctionTask(lambda: shm.deadman_settings.enabled.set(True)),
))
def SwayOnlySearch(speed=0.3, width=2.5, right_first=True):
direction = 1 if right_first else -1
return Sequential(
Log('what'),
Timed(VelocityY(direction*speed), width/(2*speed)),
Timed(VelocityY(-direction*speed), width/(speed)),
Timed(VelocityY(direction*speed), width/(2*speed)),
Zero())
def on_first_run(self, vision, amlan, *args, **kwargs):
self.use_task(Zeroed(Sequential(
Log('Aligning Amlan'),
AlignAmlan(
vision,
vision.target_bin,
amlan,
Depth(constants.above_bin_depth),
),
Log('Moving to handle grab depth'),
Timed(Depth(constants.grab_depth), self.GRAB_TIME),
Log('Extending Amlan'),
amlan.Extend(),
Timer(2),
)))
def Extend(self):
# return SetActuators([self.extend_piston], [self.retract_piston])
return Sequential(
Log('Extending {} Amlan'.format(self.name)),
SetActuators([], [self.retract_piston]),
Timer(0.3),
SetActuators([self.extend_piston], []),
)
def on_first_run(self, *args, **kwargs):
self.use_task(
WithQuaternionControl(
Sequential(
Log('Time to celebrate!'),
Depth(3, error=0.2),
Pitch(90, error=5),
Log('Weeeeeeeeeee'),
MasterConcurrent(
self.CheckDepth(),
RelativeToCurrentDepth(-0.25),
RelativeToCurrentHeading(10),
),
FunctionTask(
lambda: shm.settings_control.enabled.set(False)),
Zero(),
)))
def one_pipe(grp):
return Timeout(
45,
Sequential(
Log('Going to Search Depth'), Depth(PIPE_SEARCH_DEPTH), Zero(),
Log('Sway searching for pipe with Behind'),
TrackMovementY(search_task_behind()),
Retry(
lambda: Sequential(
Zero(),
Log('Sway searching for pipe; may have been lost'),
TrackMovementY(search_task(), shm.jank_pos.y.get()),
Log('Centering on pipe'),
Conditional(
center(),
on_fail=
Fail(
Sequential(
Log("Pipe lost, Attempting to Restore Y pos"),
Zero(),
TrackMovementY(RestorePosY(.3),
shm.jank_pos.y.get()),
))),
Depth(PIPE_FOLLOW_DEPTH),
Log('Aligning with pipe'),
Concurrent(align(), center(), finite=False),
), float("inf")), Zero(), Log('Aligned, moving forward'),
Timed(VelocityX(.4), 3),
Zero()
#Depth(PIPE_FOLLOW_DEPTH)
))
def Retract(self):
# return SetActuators([self.retract_piston], [self.extend_piston])
return Sequential(
# THE THIRD PISTON STATE
Log('Retracting {} Amlan'.format(self.name)),
SetActuators([self.extend_piston, self.retract_piston], []),
Timer(1),
SetActuators([self.retract_piston], [self.extend_piston]),
self._RemoveTube())
def grab_task(tube, amlan, tries):
return Sequential(
Log('Trying to grab {} tube with {} Amlan'.format(
constants.colors[tube].name,
amlan.name,
)),
Retry(lambda: MoveAboveTower(vision), 3),
Retry(lambda: GetThisTube(vision, tube, amlan), tries),
)
def on_first_run(self, vision, initial_tubes, *args, **kwargs):
if initial_tubes > 1:
self.downward_target = Sequential(
CenterCentroid(lambda: vision.coords(vision.tubes)),
Zero(),
)
else:
self.downward_target = Succeed(
Log('Not targeting tubes to verify grab, none should remain'))
def on_first_run(self, *args, **kwargs):
self.killed = shm.switches.hard_kill.get()
self.use_task(
Sequential(
FunctionTask(lambda: shm.switches.soft_kill.set(1)),
FunctionTask(lambda: shm.deadman_settings.enabled.set(False)),
Log('Disabling Record vision module'),
FunctionTask(lambda: shm.vision_modules.Record.set(0)),
WaitForUnkill(killed=False, wait=.5),
WaitForUnkill(),
Log('Zeroing'),
Zero(),
FunctionTask(lambda: shm.switches.soft_kill.set(0)),
EnableController(),
Heading(0),
Log('Enabling Record vision module'),
FunctionTask(lambda: shm.vision_modules.Record.set(1)),
))
