def on_first_run(self, vision, *args, **kwargs):
check = lambda: checkAligned(vision)
self.use_task(
Conditional(
While(
lambda: Sequential(
Log("Restoring depth"),
Depth(constants.WIRE_DEPTH),
Log('Align Heading'),
AlignHeading(vision),
Zero(),
Log('Align Sway'),
AlignSway(vision),
Zero(),
Log('Align Heading'),
AlignHeading(vision),
Zero(),
Log('Align Fore'),
AlignFore(vision),
Zero(),
), lambda: checkNotAligned(vision)
),
on_fail=Sequential(
Log('Lost the gate, backing up and looking again'),
Succeed(Concurrent(
Fail(Timed(VelocityX(-.4), 6)),
Fail(IdentifyGate(vision)),
)),
Fail(),
)
)
)
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, tasks, *args, **kwargs):
#tasks.insert(0, BeginMission)
tasks.append(EndMission)
self.use_task(
Retry(
lambda: Sequential(
RunTask(BeginMission),
Concurrent(
Fail(Sequential(subtasks=[RunTask(t)
for t in tasks], )),
Fail(WaitForUnkill(killed=False, wait=1)),
),
), float('inf')))
def LogSuccess(task):
name = task.__class__.__name__
return Conditional(
task,
Log('{} succeeded!'.format(name)),
Fail(Log('{} failed!'.format(name))),
)
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 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 on_first_run(self, task, time, *args, **kwargs):
# Client can check if task timed out with
# timout.timer.has_ever_finished
self.timer = Timer(time)
self.use_task(MasterConcurrent(
task,
Fail(Sequential(
self.timer,
Log('{} timed out!'.format(task.__class__.__name__)),
)),
))
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, single_bin, *args, **kwargs):
self.use_task(Sequential(
Log('Retracting Amlan'),
AMLANS[0].FastRetract(),
Conditional(
Retry(lambda: ClassifyBins(vision, single_bin), 3),
on_fail=Fail(Log('Failed to ever classify bins')),
),
Conditional(
Retry(lambda: Uncover(vision), 3),
on_fail=Fail(Log('Failed to ever remove cover')),
),
Conditional(
Retry(lambda: Drop(vision), 3),
on_fail=Fail(Log('Failed to ever accurately drop markers')),
),
))
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, vision, *args, **kwargs):
self.use_task(Conditional(
Except(Sequential(
Log('Starting bins'),
Log('Attempting bins assuming both are visible'),
Conditional(
TryBins(vision, single_bin=False),
on_fail=Sequential(
Log('Attempting bins assuming only one is visible'),
TryBins(vision, single_bin=True),
),
)
), Fail(), GlobalTimeoutError),
Log('Bins success! :O'),
Fail(Sequential(Zero(), FastDrop(), Log('Bins failure! :('))),
))
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, *args, **kwargs):
initial_heading = shm.kalman.heading.get()
depth_set = DepthRestore()
self.use_task(
Conditional(
Sequential(
MasterConcurrent(
Sequential(
Retry(lambda: Sequential(
Log('Returning to initial heading'),
Heading(initial_heading),
Log('Going to depth'),
depth_set,
#Log('Moving forward away last pos'),
#Timed(VelocityX(0.5), 1),
#Zero(),
Log('Searching for gate'),
MasterConcurrent(
IdentifyGate(vision),
VelocityHeadingSearch(initial_heading=initial_heading),
),
Zero(),
Log('Found gate, aligning'),
AlignChannel(vision),
), float('inf')),
),
Fail(Timer(180)),
),
Log('Aligned to gate, moving closer and fixing depth'),
MoveCloser(2),
Log('Beginning spin'),
StyleSegmentedSpin(),
),
Sequential(
Log('Wire completed successfully!'),
Timed(VelocityX(.4), 2),
Zero(),
RelativeToInitialHeading(180),
),
Log('Traveled too far without task completion'),
)
)
def on_first_run(self, vision, *args, **kwargs):
# Sort available tubes in priority order
has_tubes = get_shm_array(shm.recovery_world_tower, 'has_tube')
tubes = [i for i, t in enumerate(has_tubes) if t]
tubes.sort()
def amlan_avail(amlan):
return amlan.shm_tube.get() == -1 and not amlan.broken
# Try grabbing with different amlans if failing
primary_amlan, alt_amlan = tuple(AMLANS)
if not primary_amlan.available():
primary_amlan = alt_amlan
if not alt_amlan.available():
if primary_amlan is alt_amlan:
self.loge('Cannot get a tube, each amlan full or broken')
self.finish(success=False)
return
alt_amlan = primary_amlan
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),
)
# Try for highest priority tubes first, then lower ones
subtasks = []
for tube in tubes:
subtasks.append(
grab_task(
tube,
primary_amlan,
self.TRIES_PER_TUBE // 2,
))
subtasks.append(
grab_task(
tube,
alt_amlan,
self.TRIES_PER_TUBE - self.TRIES_PER_TUBE // 2,
))
subtasks.append(Fail(Log('Failed to pick up tubes')))
self.use_task(Disjunction(subtasks=subtasks))
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 TestMasterConcurrent5():
# Should fail
return LogSuccess(MasterConcurrent(
Fail(Log('master task')),
Log('slave task'),
))
surfaces=True,
timeout=timeouts['surface'],
)
track_pinger = lambda: MissionTask(name='Track',
cls=lambda: TrackPinger(depth=1.5),
modules=[],
surfaces=False,
timeout=timeouts['track_pinger'])
SearchTorpedoes = lambda: Retry(lambda: Sequential(
Retry(lambda: Conditional(
SearchFor(TrackPinger(speed=0.25),
shm.torpedoes_stake.board_visible.get,
consistent_frames=(24, 42)),
on_fail=Conditional(SearchBoard(), on_fail=Fail(GoToMarker('gate')))),
attempts=float('inf')), Stake()),
attempts=float('inf'))
TestSearch = lambda: Sequential(
SetMarker('gate'),
SearchTorpedoes(),
)
search_torpedoes = lambda: MissionTask(name='SearchTorpedoes',
cls=SearchTorpedoes,
modules=[shm.vision_modules.Stake],
surfaces=False,
timeout=timeouts['search_torpedoes'])
# TODO: Recovery + Pinger
def get_pinger_task():
global pinger_task
if pinger_task is None:
return Fail()
return pinger_task
Timeout(
SearchFor(SwayOnlySearch(right_first=get_sway_direction()),
call_buoy_visible,
consistent_frames=(0.5 * 60, 1.0 * 60)), 20),
FunctionTask(set_last_seen),
Zero(),
)
# Back up, find the triangle buoy again and use it to find the called side
ReSearch = lambda: Sequential(SearchTriangleOnFail(), AlignAnyNormal(),
SearchCalled())
# Decorator that wraps a task to search for the called side of the buoy if it fails
withReSearchCalledOnFail = lambda task: lambda: Retry(lambda: \
Conditional(main_task=task(), on_fail= \
Fail(TinySearch())), attempts=2)
# The final fallback case. If the called side cannot be found, attempt to ram any side of the triangular buoy if possible.
RamAnything = lambda backspeed=0.2, backtime=10: Sequential(
Log('Failed, backing up'), Zero(), Timeout(SearchTriangle(), 200),
AlignAnyNormal(), ApproachAny(), RamV())
# Decorator that wraps a task to Ram Anything if it fails
withRamAnythingOnFail = lambda task: lambda: Conditional(
main_task=Timeout(task(), 100), on_fail=RamAnything())
# Backs up and search for the triangular buoy again
SearchTriangleOnFail = lambda backspeed=0.2, backtime=10: Sequential(
Log('Failed, backing up'), Zero(), Timed(VelocityX(-backspeed), backtime),
Zero(), Timeout(SearchTriangle(), 120))
def withApproachRightHoleOnFail(task):
return lambda *args, **kwargs: Retry(lambda: Conditional(Timeout(task(*args, **kwargs), 120), on_fail=Fail(Sequential(Zero(), Conditional(ReSearchRightHoleOnFail(), on_fail=ApproachRightHole())))), attempts=2)
def withShootRightOnFail(task):
return lambda: Conditional(task(), on_fail=Fail(Sequential(Zero(), ApproachAlign(), ApproachRightHole() if MOVE_DIRECTION == 1 else ApproachLeftHole(), ApproachCloseRight() if MOVE_DIRECTION == 1 else ApproachCloseRight(), FireActuator('bottom_torpedo', 0.3), Backup())))
def withApproachAlignOnFail(task):
return lambda *args, **kwargs: Retry(lambda: Conditional(Timeout(task(*args, **kwargs), 60), on_fail=Fail(Sequential(Zero(), ApproachAlign(), Zero()))), attempts=2)
def withReSearchBoardOnFail(task):
return lambda *args, **kwargs: Retry(lambda: Conditional(Timeout(task(*args, **kwargs), 60), on_fail=Fail(Sequential(Zero(), ReSearchBoardOnFail()))), attempts=2)
total=3,
result=False,
invert=True),
),
Conditional(
FirstPipeGroupFirst(bend_right),
on_success=FollowPipe(shm.path_results.angle_1, shm.
path_results.angle_2),
on_fail=FollowPipe(shm.path_results.angle_2, shm.
path_results.angle_1)),
),
on_success=Sequential(
Timed(VelocityX(.1), settings.post_dist),
Log("Done!"),
Zero(),
Log("Finished path!"),
),
on_fail=Fail(
Sequential(
Log("Lost sight of path. Backing up..."),
FakeMoveX(-settings.failure_back_up_dist,
speed=settings.failure_back_up_speed),
), ),
),
),
attempts=5))
path = FullPipe()
get_path = lambda bend_right: FullPipe(bend_right)
Defer(
main_task=Either(
Sequential(
# Get at least a little bit away first
FakeMoveX(dist=-0.3, speed=0.2),
MasterConcurrent(
Consistent(lambda: shm_vars[num].visible.get(),
count=1,
total=1.5,
result=True,
invert=False),
VelocityX(-speed),
),
),
Fail(
Timer(timeout), # don't back up too far
),
),
deferred=Sequential(VelocityX(0), ),
),
on_success=Succeed(Sequential(NoOp(), ), ),
on_fail=Conditional(
FunctionTask(lambda: num == 0),
on_success=Sequential(
Log('Timed out, searching for buoy again'),
SearchBuoy(num=num, count=4, total=5),
),
# If this is the second buoy, then don't search again goddamit
on_fail=NoOp(),
))
target=lambda: shm.gate.img_width.get() / 2,
p=0.2,
deadband=alignment_tolerance_fraction,
output_function=RelativeToCurrentHeading(),
negate=True
),
finite=False
),
condition=lambda: gate_elems() < 3 or is_aligned()
)
),
Conditional(
main_task=FunctionTask(lambda: gate_elems() < 3),
on_success=Sequential(
Log('cannot see all gate_elem'),
Fail()
),
on_fail=Log('is aligned to 3 elems')
),
# finite=False
)
align_on_passageway = lambda:\
ConsistentTask(
PIDLoop(
input_value=lambda: (shm.gate.leftmost_x.get() + shm.gate.middle_x.get()) / 2,
target=lambda: shm.gate.img_width.get() / 2,
p=0.002,
output_function=VelocityY(),
negate=True
),
def Fire():
try:
fire = loaded_actuators.pop()
return Sequential(Log('firing %s' % fire), FireActuator(fire, 0.3))
except KeyError:
return Fail(Log('no more torpedoes!'))
GrabVampireClosedCoffin = lambda: \
Sequential(
Search(visible_closed),
Center(center_closed, visible_closed),
SetMarker('before_grab'),
Align(center_closed, angle_offset_closed, visible_closed),
Center(offset_closed, visible_closed, db=10),
MasterConcurrent(
Sequential(
Timer(15),
_Grab()),
RelativeToCurrentDepth(DESCEND_DEPTH, error=0.2),
),
RelativeToInitialDepth(-LID_DEPTH_1, error=0.25),
Log('what'),
Conditional(Yike(), on_fail=Fail(_Release())),
GrabVampireOpenCoffin()
)
Yike = lambda: \
Sequential(
MasterConcurrent(
Sequential(Timed(RelativeToCurrentDepth(-LID_DEPTH), 3.5), RelativeToCurrentDepth(0)),
VelocityY(0.2 * direction_closed())
),
Timed(VelocityY(0.3), 3),
Depth(SEARCH_DEPTH, error=0.2),
GoToMarker('before_grab'),
Timeout(Consistent(visible_open, count=1.5, total=2.0, invert=False, result=True), 10),
Log('Opened Coffin Successfully'),
UnsetMarker('before_grab'),
def go_to(self, marker, deadband=0.1):
target = self.get(marker)
if target is not None:
return Sequential(Log('Moving to marker {} at {}'.format(marker, target)), MoveNE(target, deadband))
return Sequential(Log('Going to nonexistent marker {}'.format(marker)), Fail())