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