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 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 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, 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, time, *args, **kwargs):
self.use_task(Sequential(
Depth(constants.WIRE_THRU_DEPTH, error=.1),
Timed(VelocityX(.3), time),
Zero(),
#Timed(VelocityY(.2), 1),
Zero()
))
def SearchCalled():
return Sequential(
Log('Searching for any buoy'),
Zero(),
SearchFor(
While(lambda: VelocityY(DIRECTION * -0.2), True),
triangle_visible,
consistent_frames=(3, 5) #TODO: Check consistent frames
),
FunctionTask(set_last_seen),
Zero())
def Spin():
return Sequential(
Timed(VelocityX(1), segment_time), Zero(),
Heading(initial_heading + 90),
Timed(VelocityY(-1), segment_time), Zero(),
Heading(initial_heading + 180),
Timed(VelocityX(-1), segment_time), Zero(),
Heading(initial_heading + 270),
Timed(VelocityY(1), segment_time), Zero(),
Heading(initial_heading),
)
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,
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, 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 on_first_run(self, clockwise=True, steps=5, spins=2, *args, **kwargs):
delta_heading = [-1, 1][clockwise] * 45
subspins = [MasterConcurrent(CheckSpinCompletion(2),
RelativeToCurrentHeading(delta_heading))]
self.use_task(Sequential(
Timed(VelocityX(1), 1), Zero(),
MasterConcurrent(
Sequential(subtasks=subspins),
VelocityX(lambda: 2 * math.cos(math.radians(shm.kalman.heading.get()))),
VelocityY(lambda: 10 * math.sin(math.radians(shm.kalman.heading.get()))),
),
Zero(),
))
def on_first_run(self, *args, **kwargs):
self.killed = shm.switches.hard_kill.get()
self.use_task(
Sequential(
VisionFramePeriod(0.1), # reset this
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)),
#AllLeds('orange'),
#Log('Wating for alignment...'),
#WaitForUnkill(wait=1.0),
#ZeroHeading(),
#Log('Aligned heading!'),
#AllLeds('cyan'),
# Need a swimmer to do this
Log('Waiting for re-kill...'),
WaitForUnkill(killed=False, wait=0.5),
#AllLeds('blue'),
Log('Waiting for unkill signal to start mission...'),
WaitForUnkill(wait=5.0),
Log('Starting mission!'),
#AllLeds('red'),
Log('Zeroing'),
ZeroHeading(),
Zero(),
FunctionTask(lambda: shm.switches.soft_kill.set(0)),
EnableController(),
Heading(0), # This will revert to the aligned heading
Log('Enabling Record vision module'),
FunctionTask(lambda: shm.vision_modules.Record.set(1)),
))
def on_run(self, vel, *args, **kwargs):
if abs(shm.jank_pos.x.get()) > vel:
self.task()
else:
self.logi("X pos restored, I hope")
Zero()
self.finish()
def cleanup():
Zero()()
release_lock()
shm.vision_modules.set(vision_state)
shm.settings_control.set(control_settings)
shm.navigation_settings.set(navigation_settings)
print('')
logger(
'Mission completed or interrupted. Restored running vision module state and zeroed submarine desires.',
copy_to_stdout=True)
end_time = time.time()
duration = end_time - start_time
logger("Mission finished in %i seconds!" % \
(duration), copy_to_stdout = True)
if initially_killed and was_ever_unkilled and shm.switches.hard_kill.get():
reload_self()
if not args.no_record:
logger('Disabling "Record" vision module', copy_to_stdout=True)
shm.vision_modules.Debug.set(False)
if not initially_recording:
shm.vision_modules.Record.set(False)
# Stop shmlogging
shmlog_proc.kill()
active_mission = shm.active_mission.get()
if not initially_recording:
active_mission.active = False
active_mission.log_path = bytes("", encoding="utf-8")
active_mission.name = bytes("", encoding="utf-8")
shm.active_mission.set(active_mission)
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 pitch_pipe(grp):
return Sequential(
Depth(PIPE_SEARCH_DEPTH),
pitch_search_task(),
Zero(),
center(),
Pitch(0),
center(),
Depth(PIPE_FOLLOW_DEPTH),
Concurrent(
#center(),
align(),
finite=False,
),
PositionalControl(),
Zero(),
)
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_finish(self):
#self.logi("Task {} was finished!".format(self.task.name))
self.logv('{} task finished in {} seconds!'.format(
self.task.name, self.this_run_time - self.first_run_time))
Zero()()
killAllModules(self.logi)
EnableController()()
shm.settings_control.quat_pid.set(False)
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 deadman_trigger():
with open('/tmp/auv-deadman', 'w') as f:
print('Timeout at {}'.format(time.time()), file=f)
os.system('killall auv-mission-runner')
Zero()()
time.sleep(1)
shm.switches.soft_kill.set(True)
def Backup(speed=0.2):
return Sequential(
Timeout(SearchFor(
search_task=While(lambda: VelocityX(-speed), True),
visible=lambda: visible() and size() < ALIGN_SIZE,
consistent_frames=(1.7*60, 2.0*60),
), 15),
Zero(),
)
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, 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_run(self, vision, *args, **kwargs):
if vision.target_bin() == None:
self.loge('Failed to align over target bin, lost bin')
self.finish(success=False)
Zero()()
else:
self.task()
if self.task.finished:
self.finish(success=self.task.success)
def on_first_run(self, vision, *args, **kwargs):
self.use_task(Zeroed(Timeout(
Sequential(
Log('Attempting to grab handle'),
GetHandle(vision, AMLANS[0]),
Log('Carrying cover away'),
RelativeToInitialDepth(-self.PICKUP_DELTA_DEPTH, error=0.1),
Timed(VelocityX(self.SLIDE_SPEED), self.SLIDE_TIME),
Zero(),
Log('Dropping cover off here'),
AMLANS[0].Retract(),
Timer(1.5),
Log('Attempting to return to near pre-grab location'),
RelativeToInitialDepth(-1),
Timed(VelocityX(-self.SLIDE_SPEED), self.SLIDE_TIME * 5 / 6),
Zero(),
), self.TOTAL_TIMEOUT)))
def one_path(grp):
return Timeout(
90,
Sequential(
Zero(),
search_task(),
While(
lambda: Sequential(
Zero(),
Log('Centering on path'),
center(grp),
Log('Going to follow depth'),
Depth(PATH_FOLLOW_DEPTH),
Log('Aligning with path'),
Heading(pathAngle(grp.get()), deadband=0.1),
Zero(),
Timer(1),
Log(grp.angle.get()),
), lambda: checkNotAligned(grp.get())),
Log('aligned'),
Zero(),
))
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 __init__(self, validator, forward=True, *args, **kwargs):
"""
validator - a function that returns True when a buoy is found and False
otherwise.
forward - determines whether the submarine should move forward or
backward during its search
timeout - the amount of time to surge
"""
super().__init__(*args, **kwargs)
# self.logv("Starting {} task".format(self.__class__.__name__))
self.validator = validator
self.forward = forward
self.surge_task = VelocityX()
self.zero_task = Zero()
self.TIMEOUT = BUOY_SURGE_TIMEOUT
def __init__(self, validator, right=True, timeout=10, *args, **kwargs):
"""
validator - a function that returns True when a buoy is found and False
otherwise.
right - determines whether the submarine should move right or
left during its search
timeout - the amount of time to surge
"""
super().__init__(*args, **kwargs)
# self.logv("Starting {} task".format(self.__class__.__name__))
self.validator = validator
self.right = right
self.surge_task = VelocityY()
self.zero_task = Zero()
self.depth_task = Depth(constants.BUOY_SEARCH_DEPTH)
self.TIMEOUT = timeout
