class FirstPipeGroupFirst(Task): # Checks whether the first pipe group in shm is the first pipe we should follow. # Succeeds if the first pipe group is consistently the right one, fails otherwise def on_first_run(self, bend_right): self.angle_1_checker = ConsistencyCheck(6, 8) self.angle_2_checker = ConsistencyCheck(6, 8) def on_run(self, bend_right): angle_1 = shm.path_results.angle_1.get() angle_2 = shm.path_results.angle_2.get() v1 = np.complex128([np.exp(1j * angle_1)]).view(np.float64) v2 = np.complex128([np.exp(1j * angle_2)]).view(np.float64) cp = np.cross(v1, v2) cond = (cp > 0) ^ bend_right if self.angle_1_checker.check(cond): self.finish() if self.angle_2_checker.check(not cond): self.finish(success=False) return diff = math.atan( math.sin((angle_2 - angle_1)) / math.cos((angle_2 - angle_1))) # TODO this might not be working print(angle_1, angle_2, diff) if self.angle_1_checker.check( diff > 0 ^ (angle_1 < angle_2) ^ (not bend_right)): self.finish() if self.angle_2_checker.check( diff < 0 ^ (angle_1 < angle_2) ^ (not bend_right)): self.finish(success=False)
def on_first_run(self): self.update_data() self.align = Heading( lambda: self.pipe_results.angle + shm.kalman.heading.get(), deadband=0.5) self.alignment_checker = ConsistencyCheck(19, 20) pipe_found = self.pipe_results.heuristic_score > 0 self.center = DownwardTarget(lambda self=self: (self.pipe_results.center_x * 100, self. pipe_results.center_y * 100), target=(0, 0), deadband=(1, 1), px=0.03, py=0.03, dx=0.01, dy=0.01, valid=pipe_found) self.logi("Beginning to align to the pipe's heading") c_center = "Camera Center: " + str((.5, .5)) self.logi("Pipe center" + str(self.pipe_results.center_x) + " " + str(self.pipe_results.center_y)) self.logi(c_center)
def __init__(self, validator, locator_task, target_coords, vision_group, heading_task, forward_target_p=0.001, *args, **kwargs): """ validator - a function that returns True when the target is visible and False otherwise. locator_task - a task that locates the target target_coords - the coordinates of the target with which to align vision_group - the shm group for the buoy """ super().__init__(*args, **kwargs) self.validator = validator self.locator_task = locator_task self.heading_task = heading_task def get_center(): return (0, 0) # TODO use normalized coordinates instead self.target_task = ForwardTarget(target_coords, target=get_center, px=forward_target_p, dx=forward_target_p / 2, py=forward_target_p, dy=forward_target_p / 2, deadband=(30, 30)) self.target_checker = ConsistencyCheck(5, 5, strict=True) self.TIMEOUT = 60
def on_first_run(self): assert isinstance(self.track_method, TrackMethod) self.follow_task = None self.heading_to_pinger = None shm.hydrophones_settings.track_frequency_target.set(PINGER_FREQUENCY) shm.hydrophones_settings.track_magnitude_threshold.set( TRACK_MAG_THRESH) shm.hydrophones_settings.track_cooldown_samples.set(795000) shm.navigation_settings.position_controls.set(1) shm.navigation_settings.optimize.set(0) self.localizer = Localizer(PINGER_FREQUENCY) self.has_made_progress = False self.hydro_watcher = shm.watchers.watcher() self.hydro_watcher.watch(shm.hydrophones_results_track) self.time_since_last_ping = self.this_run_time self.pinger_positions = collections.deque(maxlen=7) self.silencer = ThrusterSilencer() self.pinger = None self.ping_deviating_checker = ConsistencyCheck(3, 4) self.ontop_checker = ConsistencyCheck(3, 4) self.pings = []
def on_first_run(self, altitude, *args, **kwargs): self.min_speed = 0.008 self.min_delta = 0.1 self.deque_size = 20 self.altitude_task = Altitude(altitude, p=0.3) self.stop_cons_check = ConsistencyCheck(3, 3) self.readings = deque() self.initial_altitude = shm.dvl.savg_altitude.get() self.last_altitude = self.initial_altitude
def on_first_run(self): self.update_data() pipe_found = self.pipe_results.heuristic_score > 0 self.centered_checker = ConsistencyCheck(8, 10) self.center = DownwardTarget(lambda self=self: (self.pipe_results.center_x, self.pipe_results.center_y), target=get_downward_camera_center, deadband=(30,30), px=0.002, py=0.002, dx=0.002, dy=0.002, valid=pipe_found)
def on_first_run(self): self.update_data() self.align = Heading(lambda: self.pipe_results.angle + shm.kalman.heading.get(), deadband=0.5) self.alignment_checker = ConsistencyCheck(49, 50) pipe_found = self.pipe_results.heuristic_score > 0 self.center = DownwardTarget(lambda self=self: (self.pipe_results.center_x, self.pipe_results.center_y), target=get_downward_camera_center, deadband=(10,10), px=0.001, py=0.001, dx=0.002, dy=0.002, valid=pipe_found)
class DualConsistency(Task): """edited from will_common""" def on_first_run(self, test_success, test_fail, count, total, invert): # Multiply by 60 to specify values in seconds, not ticks self.checker_success = ConsistencyCheck(count*60, total*60, default=False) self.checker_fail = ConsistencyCheck(count*60, total*60) def on_run(self, test_success, test_fail, count, total, invert): test_result_success = call_if_function(test_success) test_result_fail = call_if_function(test_fail) if self.checker_success.check(not test_result_success if invert else test_result_success): self.finish(success=True) elif self.checker_fail.check(not test_result_fail if invert else test_result_fail): self.finish(success=False)
class IdentifyGate(Task): """ Finish when we can see a good enough amount of the gate """ min_bbar_width = 0.15 def on_first_run(self, vision): self.seen_cons_check = ConsistencyCheck(4, 5) def on_run(self, vision): self.seen_cons_check.add(vision.bottom is not None and \ # (vision.left is not None or vision.right is not None) and \ bbar_width_ratio(vision) >= self.min_bbar_width) if self.seen_cons_check.check(): self.finish()
class IdentifyGate(Task): """ Finish when we can see a good enough amount of the gate """ min_bbar_width = 0.3 def on_first_run(self, vision): self.seen_cons_check = ConsistencyCheck(4, 5) def on_run(self, vision): self.seen_cons_check.add(vision.bottom is not None and \ # (vision.left is not None or vision.right is not None) and \ bbar_width_ratio(vision) >= self.min_bbar_width) if self.seen_cons_check.check(): self.finish()
class center(Task): def update_data(self): self.pipe_results = shm.pipe_results.get() def on_first_run(self): self.update_data() pipe_found = self.pipe_results.heuristic_score > 0 self.centered_checker = ConsistencyCheck(8, 10) self.center = DownwardTarget( lambda self=self: (self.pipe_results.center_x, self.pipe_results.center_y), target=get_downward_camera_center, deadband=(30, 30), px=0.002, py=0.002, dx=0.002, dy=0.002, valid=pipe_found) #self.logi("Beginning to center on the pipe") def on_run(self): self.update_data() self.center() #self.logi("Results Y: {}".format(str(self.pipe_results.center_y))) #self.logi("Center: {}".format(str(get_downward_camera_center()[1]))) if not check_seen(): self.finish(success=False) if self.centered_checker.check(self.center.finished): self.center.stop() self.finish()
class AltitudeUntilStop(Task): """ Attempt to move to the target altitude until the sub gets stuck """ def on_first_run(self, altitude, *args, **kwargs): self.min_speed = 0.008 self.min_delta = 0.1 self.deque_size = 20 self.altitude_task = Altitude(altitude, p=0.3) self.stop_cons_check = ConsistencyCheck(3, 3) self.readings = deque() self.initial_altitude = shm.dvl.savg_altitude.get() self.last_altitude = self.initial_altitude def on_run(self, altitude, *args, **kwargs): if not self.altitude_task.has_ever_finished: self.altitude_task() current_altitude = shm.dvl.savg_altitude.get() self.readings.append((current_altitude, time.time())) if len(self.readings) > self.deque_size: self.readings.popleft() if abs(current_altitude - self.initial_altitude) >= self.min_delta and \ len(self.readings) >= self.deque_size: delta_altitude = self.readings[-1][0] - self.readings[0][0] delta_time = self.readings[-1][1] - self.readings[0][1] speed = abs(delta_altitude / delta_time) if self.stop_cons_check.check(speed < self.min_speed): self.logi('Stopped changing altitude, finishing') self.finish() else: self.loge('Bounding altitude reached') self.finish(success=False)
class center(Task): def update_data(self): self.pipe_results = shm.pipe_results.get() def on_first_run(self): self.update_data() pipe_found = self.pipe_results.heuristic_score > 0 self.centered_checker = ConsistencyCheck(18, 20) self.center = DownwardTarget(lambda self=self: (self.pipe_results.center_x * 100, self. pipe_results.center_y * 100), target=(0, 0), deadband=(4, 4), px=0.03, py=0.03, dx=0.01, dy=0.01, valid=pipe_found) self.logi("Beginning to center on the pipe") self.logi("Pipe center" + str(self.pipe_results.center_x) + " " + str(self.pipe_results.center_y)) def on_run(self): self.update_data() self.center() if self.centered_checker.check(self.center.finished): self.center.stop() self.finish()
class first_center(Task): def on_first_run(self, results): self.update_data(results) path_found = self.path_results.visible > 0 self.centered_checker = ConsistencyCheck(8, 10) self.center = DownwardTarget( lambda self=self: (self.path_results.center_x, self.path_results.center_y), target=(0, 0), deadband=(.05, .05), px=0.5, py=0.5, dx=0.02, dy=0.02, valid=path_found) def on_run(self, results): self.update_data(results) self.center() # if not check_seen(results): # self.finish(success=False) if self.centered_checker.check(self.center.finished): self.center.stop() self.finish()
class align(Task): def update_data(self): self.pipe_results = shm.pipe_results.get() def on_first_run(self): self.update_data() self.align = Heading(lambda: self.pipe_results.angle + shm.kalman.heading.get(), deadband=0.5) self.alignment_checker = ConsistencyCheck(19, 20) pipe_found = self.pipe_results.heuristic_score > 0 self.center = DownwardTarget(lambda self=self: (self.pipe_results.center_x, self.pipe_results.center_y), target=lambda self=self: get_camera_center(self.pipe_results), deadband=(20,20), px=0.003, py=0.003, dx=0.001, dy=0.001, valid=pipe_found) self.logi("Beginning to align to the pipe's heading") def on_run(self): self.update_data() self.align() self.center() if self.alignment_checker.check(self.align.finished): self.finish()
class align(Task): def update_data(self): self.pipe_results = shm.pipe_results.get() def on_first_run(self): self.update_data() self.align = Heading(lambda: self.pipe_results.angle + shm.kalman.heading.get(), deadband=0.5) self.alignment_checker = ConsistencyCheck(49, 50) pipe_found = self.pipe_results.heuristic_score > 0 self.center = DownwardTarget(lambda self=self: (self.pipe_results.center_x, self.pipe_results.center_y), target=get_downward_camera_center, deadband=(10,10), px=0.001, py=0.001, dx=0.002, dy=0.002, valid=pipe_found) #self.logi("Beginning to align to the pipe's heading") def on_run(self): self.update_data() self.align() self.center() if not check_seen(): self.finish(success=False) if self.alignment_checker.check(self.align.finished): self.finish()
class IdentifyBins(Task): """Identifies which bin to drop markers into, centers over it Start: Both bins visible in downward cam Finish: Centered over chosen bin """ def on_first_run(self, bin_group, heading=None, *args, **kwargs): self.logi("Centering over bins...") self.logv("Starting IdentifyBins task") self.task = DownwardTarget(px=0.0025, py=0.0025) self.align_checker = ConsistencyCheck(6, 6) # TODO start alignment task. self.init_time = self.this_run_time self.bin_group = bin_group if bin1.covered == FAST_RUN: self.target_bin = bin1 else: self.target_bin = bin2 def on_run(self, bin_group, heading=None): self.bin1_results = self.bin_group.get() target = get_camera_center(self.bin1_results) # TODO Increase deadband as time increases. self.task((self.bin1_results.x, self.bin1_results.y), target=target, deadband=(25, 25), valid=lambda: self.bin1_results.probability > 0.0) if self.task.finished: if heading is None: target_heading = shm.kalman.heading.get() + self.bin1_results.angle else: target_heading = heading() align_task = Heading(target_heading, deadband=0.5) align_task() if self.align_checker.check(align_task.finished): VelocityX(0)() VelocityY(0)() self.finish() else: self.align_checker.clear() def on_finish(self): self.logi("Centered!") self.logv('IdentifyBins task finished in {} seconds!'.format( self.this_run_time - self.init_time))
def on_first_run(self, results): self.update_data(results) path_found = self.path_results.visible > 0 self.centered_checker = ConsistencyCheck(8, 10) self.center = DownwardTarget( lambda self=self: (self.path_results.center_x, self.path_results.center_y), target=(0, 0), deadband=(.05, .05), px=0.5, py=0.5, dx=0.02, dy=0.02, valid=path_found)
class Consistent(Task): def on_first_run(self, test, count, total, invert, result): # Multiple by 60 to specify in seconds self.checker = ConsistencyCheck(count * 60, total * 60, default=False) def on_run(self, test, count, total, invert, result): test_result = call_if_function(test) if self.checker.check(not test_result if invert else test_result): self.finish(success=result)
def on_first_run(self): self.search = SpiralSearch(optimize_heading=True, min_spin_radius=2.0) self.recovery_check = ConsistencyCheck(5, 5) self.bins_check = ConsistencyCheck(5, 5) self.bins_watcher = shm.watchers.watcher() self.bins_watcher.watch(shm.bin_cover) self.bins_watcher.watch(shm.bin_yellow_1) self.bins_watcher.watch(shm.bin_yellow_2) self.bins_visible = False self.recovery_watcher = shm.watchers.watcher() self.recovery_watcher.watch(shm.recovery_vision) self.recovery_visible = False self.bins = Bins() self.recovery = Recovery() self.task = None
class SearchFor(Task): def on_first_run(self, search_task, visible, consistent_frames=(3, 5)): self.found_checker = ConsistencyCheck(*consistent_frames) def on_run(self, search_task, visible, consistent_frames=(3, 5)): visible = call_if_function(visible) if self.found_checker.check(visible): self.finish() else: search_task()
class SearchFor(Task): def on_first_run(self, search_task, visible, consistent_frames=(3, 5)): self.found_checker = ConsistencyCheck(*consistent_frames) def on_run(self, search_task, visible, consistent_frames=(3, 5)): visible = call_if_function(visible) if self.found_checker.check(visible): self.finish() else: search_task()
class CheckBinsInSight(Task): """ Checks if the desired bin is in sight of the camera Used in SearchBinsTask as MasterConcurrent's end condition""" def on_first_run(self, *args, **kwargs): self.logv("Checking if bins in sight") self.init_time = self.this_run_time self.seen_checker1 = ConsistencyCheck(6, 8) self.seen_checker2 = ConsistencyCheck(6, 8) def on_run(self): cover_results = cover.get() yellow_results = yellow1.get() if self.seen_checker1.check(cover_results.probability > 0.0) or \ self.seen_checker2.check(yellow_results.probability > 0.0): self.finish() def on_finish(self): self.logv('CheckBinsInSight task finished in {} seconds!'.format( self.this_run_time - self.init_time))
class Buoy(Task): """ Wrapper around the BuoyRam class that will specifically ram a red or green buoy """ def __init__(self, buoy, *args, **kwargs): super().__init__(*args, **kwargs) # Instantiate the BuoyRam task self.buoy = buoy self.align_task = AlignTarget(self.location_validator, AssumeBuoy(self.location_validator), (self.buoy.center_x.get, self.buoy.center_y.get), self.buoy) self.ram_task = BuoyRam(self.location_validator, (self.buoy.center_x.get, self.buoy.center_y.get), self.buoy, self.collision_validator, self.align_task) self.seen_frames_checker = ConsistencyCheck(5, 5) self.last_percent_frame = 0 self.PERCENT_FRAME_THRESHOLD = 10 self.PERCENT_FRAME_DELTA_THRESHOLD = 10 self.TIMEOUT = 100 def on_first_run(self): self.logv("Starting {} task".format(self.__class__.__name__)) def on_run(self): # Perform BuoyRam task if self.this_run_time - self.first_run_time > self.TIMEOUT: self.finish() self.loge("Buoy ({}) timed out!".format(self.buoy)) return self.ram_task() if self.ram_task.finished: self.finish() def on_finish(self): self.logv("Buoy ({}) task finished in {} seconds!".format( self.buoy, self.this_run_time - self.first_run_time)) Zero()() def location_validator(self): # TODO even more robust location validator return self.seen_frames_checker.check(self.buoy.probability.get() != 0) def collision_validator(self): # TODO even more robust collision validator if not shm.gpio.wall_1.get(): self.logi("Detected buoy ram using touch sensor!") return True current = self.buoy.percent_frame.get() if current >= self.PERCENT_FRAME_THRESHOLD: if abs(self.last_percent_frame - current) <= self.PERCENT_FRAME_DELTA_THRESHOLD: return True self.last_percent_frame = current return False
def __init__(self, *args, **kwargs): super().__init__(*args, **kwargs) self.locator_task = LocateBuoy(self.location_validator) self.locator_task = AssumeBuoy(self.location_validator) self.align_task = AlignTarget(self.location_validator, self.locator_task, (yellow_buoy_results.top_x.get, yellow_buoy_results.top_y.get), yellow_buoy_results, forward_target_p=0.002) self.concurrent_align_task = AlignTarget(self.location_validator, self.locator_task, (yellow_buoy_results.top_x.get, yellow_buoy_results.top_y.get), yellow_buoy_results, forward_target_p=0.002) self.MAX_RAM_SPEED = 0.6 self.ram_speed = self.MAX_RAM_SPEED self.ram_task = RamTarget(self.location_validator, self.collision_validator, self.locator_task, self.concurrent_align_task, ram_speed=lambda: self.ram_speed) self.ready_to_ram_checker = ConsistencyCheck(40, 40) self.seen_frames_checker = ConsistencyCheck(5, 5) SCUTTLE_TIME = 2.0 BUOY_MOUNTING_DISTANCE = 0.55 self.scuttle = Sequential(Log("Rising to prepare for scuttle"), RelativeToInitialDepth(-0.2, error=0.02), Log("Moving on top of Yellow Buoy"), MoveX(BUOY_MOUNTING_DISTANCE), Log("Beginning %0.2f second scuttle!" % SCUTTLE_TIME), MasterConcurrent(Timer(SCUTTLE_TIME), VelocityX(1.0), RelativeToInitialDepth(3.0, error=0.03)), Log("Scuttle complete. Returning to targeting position."), Depth(1.0) # GoToPosition(lambda: self.target_position[0], lambda: self.target_position[1], depth=lambda: self.target_depth) ) self.tasks = Sequential(self.locator_task, self.align_task, self.ram_task, self.scuttle) #self.last_percent_frame = 0 #self.PERCENT_FRAME_DELTA_THRESHOLD = 10 self.PERCENT_FRAME_THRESHOLD = 2 self.PERCENT_FRAME_SLOWDOWN_THRESHOLD = 0.4 self.TIMEOUT = 100
class _TrackPinger(Task): def on_first_run(self, speed=0.4): self.last_shmval = None self.last_target_heading = None self.last_target_elevation = 0 self.checker = ConsistencyCheck(count=3, total=7) def update(self): self.shmval = shm.hydrophones_results_track.tracked_ping_heading.get() # print('last: ' + str(self.last_shmval) + ', new: ' + str(self.shmval)) if self.last_shmval is None or self.shmval != self.last_shmval: lltarget_heading = self.last_target_heading if self.last_target_heading is not None else self.shmval self.last_shmval = self.shmval self.last_target_heading = self.shmval self.last_target_elevation = shm.hydrophones_results_track.tracked_ping_elevation.get( ) self.log('heading: ' + str(self.last_target_heading), level='info') self.log('elevation: ' + str(self.last_target_elevation), level='info') return self.checker.check( self.angle_diff(self.get_target_heading(), lltarget_heading) > 90) return False def get_target_heading(self): return self.last_target_heading def get_target_elevation(self): return self.last_target_elevation def angle_diff(self, a, b): return math.degrees( abs( math.atan2(math.sin(math.radians(a) - math.radians(b)), math.cos(math.radians(a) - math.radians(b))))) def calc_speed(self, speed): diff = self.angle_diff(self.get_target_heading(), shm.kalman.heading.get()) print("diff: " + str(diff)) if diff < 45: print(min((45 - diff) / 20, speed)) return min((45 - diff) / 20, speed) else: print("0") return 0 def on_run(self, speed): if self.update(): VelocityX(0)() self.finish() return Heading(self.get_target_heading())() VelocityX(self.calc_speed(speed))()
class ConsistentTask(Task): """ Finishes when a non-finite task is consistently finished """ def on_first_run(self, task, success=18, total=20, *args, **kwargs): self.cons_check = ConsistencyCheck(success, total) def on_run(self, task, *args, **kwargs): task() if self.cons_check.check(task.finished): self.finish()
def on_first_run(self, altitude, *args, **kwargs): self.min_speed = 0.008 self.min_delta = 0.1 self.deque_size = 20 self.success = False self.altitude_task = Altitude(altitude, p=0.3) self.stop_cons_check = ConsistencyCheck(3, 3) self.readings = deque() self.initial_altitude = shm.dvl.savg_altitude.get() self.last_altitude = self.initial_altitude
class ConsistentTask(Task): """ Checks to make sure a non-finite task consistently is finished """ def on_first_run(self, task, success=18, total=20, *args, **kwargs): self.cons_check = ConsistencyCheck(success, total) def on_run(self, task, *args, **kwargs): task() if self.cons_check.check(task.finished): self.finish()
def on_first_run(self): self.update_data() pipe_found = self.pipe_results.heuristic_score > 0 self.centered_checker = ConsistencyCheck(18, 20) self.center = DownwardTarget(lambda self=self: (self.pipe_results.center_x, self.pipe_results.center_y), target=lambda self=self: get_camera_center(self.pipe_results), deadband=(30,30), px=0.003, py=0.003, dx=0.001, dy=0.001, valid=pipe_found) self.logi("Beginning to center on the pipe")
class align(Task): def on_first_run(self): self.align = PIDLoop(output_function=RelativeToCurrentHeading(), target=0, input_value=lambda: shm.pipe_results.angle.get(), negate=True, deadband=1) self.alignment_checker = ConsistencyCheck(19, 20) self.logi("Beginning to align to the pipe's heading") def on_run(self): self.align() if self.alignment_checker.check(self.align.finished): self.finish()
def on_first_run(self, run_type, heading=None, uncovered_bin_vector=None): self.logi("Centering over bins...") self.logv("Starting IdentifyBins task") self.center_valid = False self.center_coords = (0, 0) self.task = DownwardTarget(px=0.0025, py=0.0025, point=lambda: self.center_coords, valid=lambda: self.center_valid) self.center_checker = ConsistencyCheck(15, 17) self.align_checker = ConsistencyCheck(6, 6) # TODO start alignment task. self.init_time = self.this_run_time self.uncovered_bin_vector = None self.seen_two = False cam = get_downward_camera() self.cover_tracker = Tracker(cam['width'] * 0.15) self.yellow_tracker = Tracker(cam['width'] * 0.15)
def on_first_run(self): self.has_made_progress = True self.seen_frames_checker = ConsistencyCheck(3, 3, strict=False) def location_validator(buoy): return self.seen_frames_checker.check(buoy.probability.get() != 0) self.depth_task = DepthRestore() self.heading_task = HeadingRestore() self.up_task = DepthRestore(constants.BUOY_OVER_DEPTH) self.dodge_vel = -.4 if yellowRight else .4 self.over_task = Sequential(Timed(VelocityY(self.dodge_vel), 2), DirectionalSurge(6, .4, compensate=True)) self.heading_task = HeadingRestore() self.task = Sequential( Depth(constants.BUOY_SEARCH_DEPTH), self.heading_task, # Depth(0.9, error=.01) # SeqLog("Looking for red buoy"), LocateFirstBuoy(lambda: location_validator(red_buoy_results), forward=True, right=BUOY_RIGHT_TO_REACH[0], middle=yellow_buoy_results), Buoy(red_buoy_results, first_buoy=True, right=not redRight()), # self.depth_task, # SeqLog("Looking for green buoy stage 1"), LocateBuoyStrafe(lambda: location_validator(yellow_buoy_results), right=True, timeout=3), SeqLog("Looking for green buoy"), LocateBuoyStrafe(lambda: location_validator(green_buoy_results), right=not greenRight(), timeout=3), Buoy(green_buoy_results, right=not greenRight()), # self.depth_task, SeqLog("Looking for yellow buoy"), LocateBuoyStrafe(lambda: location_validator(yellow_buoy_results), right=not yellowRight(), timeout=2), Buoy(yellow_buoy_results, right=not yellowRight(), yellow=True), HeadingRestore(), SeqLog("Rising to Over depth"), self.up_task, SeqLog("Going around buoys"), self.over_task)
def on_first_run(self, bin_group, heading=None, *args, **kwargs): self.logi("Centering over bins...") self.logv("Starting IdentifyBins task") self.task = DownwardTarget(px=0.0025, py=0.0025) self.align_checker = ConsistencyCheck(6, 6) # TODO start alignment task. self.init_time = self.this_run_time self.bin_group = bin_group if bin1.covered == FAST_RUN: self.target_bin = bin1 else: self.target_bin = bin2
class FirstPipeGroupFirst(Task): # Checks whether the first pipe group in shm is the first pipe we should follow. # Succeeds if the first pipe group is consistently the right one, fails otherwise def on_first_run(self, bend_right): self.angle_1_checker = ConsistencyCheck(6, 8) self.angle_2_checker = ConsistencyCheck(6, 8) def on_run(self, bend_right): angle_1 = shm.path_results.angle_1.get() angle_2 = shm.path_results.angle_2.get() diff = math.atan( math.sin((angle_2 - angle_1)) / math.cos((angle_2 - angle_1))) # TODO this might not be working print(angle_1, angle_2, diff) if self.angle_1_checker.check( diff > 0 ^ (angle_1 < angle_2) ^ (not bend_right)): self.finish() if self.angle_2_checker.check( diff < 0 ^ (angle_1 < angle_2) ^ (not bend_right)): self.finish(success=False)
def __init__(self, buoy, *args, **kwargs): super().__init__(*args, **kwargs) # Instantiate the BuoyRam task self.buoy = buoy self.align_task = AlignTarget(self.location_validator, AssumeBuoy(self.location_validator), (self.buoy.center_x.get, self.buoy.center_y.get), self.buoy) self.ram_task = BuoyRam(self.location_validator, (self.buoy.center_x.get, self.buoy.center_y.get), self.buoy, self.collision_validator, self.align_task) self.seen_frames_checker = ConsistencyCheck(5, 5) self.last_percent_frame = 0 self.PERCENT_FRAME_THRESHOLD = 10 self.PERCENT_FRAME_DELTA_THRESHOLD = 10 self.TIMEOUT = 100
class AlignTarget(Task): """ Aligns using ForwardTarget on a target coordinate, while ensuring that the target is visible """ def __init__(self, validator, locator_task, target_coords, vision_group, forward_target_p=0.003, *args, **kwargs): """ validator - a function that returns True when the target is visible and False otherwise. locator_task - a task that locates the target target_coords - the coordinates of the target with which to align """ super().__init__(*args, **kwargs) self.validator = validator self.locator_task = locator_task def get_center(): return get_forward_camera_center() self.target_task = ForwardTarget(target_coords, target=get_center, px=forward_target_p, dx=forward_target_p/3, py=forward_target_p, dy=forward_target_p/3, deadband=(30, 30)) self.target_checker = ConsistencyCheck(10, 10) self.TIMEOUT = 60 def on_first_run(self): self.logv("Starting {} task".format(self.__class__.__name__)) def on_run(self): if self.this_run_time - self.first_run_time > self.TIMEOUT: self.finish() self.loge("{} timed out!".format(self.__class__.__name__)) return self.logv("Running {}".format(self.__class__.__name__)) if self.validator(): self.target_task() else: self.locator_task() if self.target_checker.check(self.target_task.finished): self.finish() def on_finish(self): self.logv('{} task finished in {} seconds!'.format( self.__class__.__name__, self.this_run_time - self.first_run_time))
def __init__(self, validator, locator_task, target_coords, vision_group, forward_target_p=0.003, *args, **kwargs): """ validator - a function that returns True when the target is visible and False otherwise. locator_task - a task that locates the target target_coords - the coordinates of the target with which to align """ super().__init__(*args, **kwargs) self.validator = validator self.locator_task = locator_task def get_center(): return get_forward_camera_center() self.target_task = ForwardTarget(target_coords, target=get_center, px=forward_target_p, dx=forward_target_p/3, py=forward_target_p, dy=forward_target_p/3, deadband=(30, 30)) self.target_checker = ConsistencyCheck(10, 10) self.TIMEOUT = 60
class ConsistentTask(Task): """ Finishes when a non-finite task is consistently finished """ def on_first_run(self, task, success=18, total=20, *args, **kwargs): self.cons_check = ConsistencyCheck(success, total) def on_run(self, task, debug=False, *args, **kwargs): task() if self.cons_check.check(task.finished): self.finish() if debug: success_char = ["x", "^"][self.cons_check.results[-1] == 1] dropped_char = ["x", "^"][self.cons_check.results[0] == 1] self.logd("{}/{}/{}/{}/{}".format( success_char, sum(x == 1 for x in self.cons_check.results), self.cons_check.count, self.cons_check.total, dropped_char, ))
class AltitudeUntilStop(Task): """ Attempt to move to the target altitude until the sub gets stuck """ def on_first_run(self, altitude, *args, **kwargs): self.min_speed = 0.008 self.min_delta = 0.1 self.deque_size = 20 self.success = False self.altitude_task = Altitude(altitude, p=0.3) self.stop_cons_check = ConsistencyCheck(3, 3) self.readings = deque() self.initial_altitude = shm.dvl.savg_altitude.get() self.last_altitude = self.initial_altitude def on_run(self, altitude, *args, **kwargs): if not self.altitude_task.has_ever_finished: self.altitude_task() current_altitude = shm.dvl.savg_altitude.get() self.readings.append((current_altitude, time.time())) if len(self.readings) > self.deque_size: self.readings.popleft() if abs(current_altitude - self.initial_altitude) >= self.min_delta and \ len(self.readings) >= self.deque_size: delta_altitude = self.readings[-1][0] - self.readings[0][0] delta_time = self.readings[-1][1] - self.readings[0][1] speed = abs(delta_altitude / delta_time) if self.stop_cons_check.check(speed < self.min_speed): self.logi('Stopped changing altitude, finishing') self.success = True self.finish() else: self.loge('Bounding altitude reached') self.finish()
class center(Task): def update_data(self): self.pipe_results = shm.pipe_results.get() def on_first_run(self): self.update_data() pipe_found = self.pipe_results.heuristic_score > 0 self.centered_checker = ConsistencyCheck(18, 20) self.center = DownwardTarget(lambda self=self: (self.pipe_results.center_x, self.pipe_results.center_y), target=get_downward_camera_center, deadband=(30,30), px=0.003, py=0.003, dx=0.001, dy=0.001, valid=pipe_found) self.logi("Beginning to center on the pipe") def on_run(self): self.update_data() self.center() if self.centered_checker.check(self.center.finished): self.center.stop() self.finish()
class ConsistentObject: """ Consistency-check an object's existence """ def __init__(self, seen_cons_check=(2, 3), unseen_cons_check=(5, 6)): self.last_obj = None self.tracking = False self.seen_cons_check = ConsistencyCheck(*seen_cons_check) self.unseen_cons_check = ConsistencyCheck(*unseen_cons_check) def map(self, obj): """ Call with a valid object to count as "seeing the object", or with None to count as "not seeing the object" """ if obj is not None: self.last_obj = obj if self.tracking: self.unseen_cons_check.add(False) else: self.seen_cons_check.add(True) if self.seen_cons_check.check(): self.tracking = True self.seen_cons_check.clear() self.unseen_cons_check.clear() else: if self.tracking: self.unseen_cons_check.add(True) if self.unseen_cons_check.check(): self.tracking = False self.seen_cons_check.clear() self.unseen_cons_check.clear() self.last_obj = None else: self.seen_cons_check.add(False) self.last_obj = None if self.tracking: return self.last_obj else: return None
def on_first_run(self, bend_right): self.angle_1_checker = ConsistencyCheck(6, 8) self.angle_2_checker = ConsistencyCheck(6, 8)
def on_first_run(self, search_task, visible, consistent_frames=(3, 5)): self.found_checker = ConsistencyCheck(*consistent_frames)
def on_first_run(self): self.has_made_progress = True self.seen_frames_checker = ConsistencyCheck(3, 3, strict=False) def location_validator(buoy): return self.seen_frames_checker.check(buoy.probability.get() != 0) self.depth_task = DepthRestore() self.heading_task = HeadingRestore() self.up_task = DepthRestore(constants.BUOY_OVER_DEPTH) self.dodge_vel = -.4 if yellowRight else .4 self.over_task = Timed(VelocityX(.4), 8) self.heading_task = HeadingRestore() self.task = Sequential( Depth(constants.BUOY_SEARCH_DEPTH), Conditional( MasterConcurrent( Sequential( self.heading_task, # Depth(0.9, error=.01) # SeqLog("Looking for red buoy"), LocateFirstBuoy(lambda: location_validator(red_buoy_results), forward=True, right=BUOY_RIGHT_TO_REACH[0], middle=yellow_buoy_results), Buoy(red_buoy_results, first_buoy=True, right=redRight()), # self.depth_task, # SeqLog("Looking for green buoy stage 1"), LocateBuoyStrafe(lambda: location_validator(yellow_buoy_results), right=True, timeout=3), SeqLog("Looking for green buoy"), LocateBuoyStrafe( lambda: location_validator(green_buoy_results), right=greenRight(), timeout=3), Buoy(green_buoy_results, right=greenRight()), # self.depth_task, SeqLog("Looking for yellow buoy"), LocateBuoyStrafe( lambda: location_validator(yellow_buoy_results), right=yellowRight(), timeout=2), Buoy(yellow_buoy_results, right=yellowRight(), yellow=True), Log("re-aligning red buoy"), LocateBuoyStrafe( lambda: location_validator(red_buoy_results), right=secondRedRight(), timeout=2), Buoy(red_buoy_results, right=secondRedRight(), yellow=False, align_only=True), ), PreventSurfacing(), ), on_success=Sequential( Zero(), self.heading_task, SeqLog("Rising to Over depth"), self.up_task, SeqLog("Going over buoys"), self.over_task, ), on_fail=Sequential( Zero(), self.heading_task, SeqLog("Going to Over depth"), self.up_task, SeqLog("Going over buoys"), self.over_task, Timed(VelocityX(.4), 8), )))
def __init__(self, buoy, right=True, first_buoy=False, yellow=False, align_only=False, *args, **kwargs): super().__init__(*args, **kwargs) # Instantiate the BuoyRam task self.buoy = buoy self.heading_task = HeadingRestore() if align_only: self.align_task = AlignTarget( self.location_validator, LocateAdjacentBuoy(self.location_validator, right=right), (self.buoy.r_side_x.get, self.buoy.center_y.get), self.buoy, self.heading_task) self.ram_task = BuoyAlign( self.location_validator, (self.buoy.r_side_x.get, self.buoy.center_y.get), self.buoy, self.collision_validator, self.align_task, right=right, first_buoy=first_buoy, yellow=yellow) elif yellow: self.align_task = AlignTarget( self.location_validator, LocateAdjacentBuoy(self.location_validator, right=right), (self.buoy.r_side_x.get, self.buoy.bottom_y.get), self.buoy, self.heading_task) self.ram_task = BuoyRam( self.location_validator, (self.buoy.r_side_x.get, self.buoy.bottom_y.get), self.buoy, self.collision_validator, self.align_task, right=right, first_buoy=first_buoy, yellow=yellow) else: self.align_task = AlignTarget( self.location_validator, LocateAdjacentBuoy(self.location_validator, right=right), (self.buoy.r_side_x.get, self.buoy.center_y.get), self.buoy, self.heading_task) self.ram_task = BuoyRam( self.location_validator, (self.buoy.r_side_x.get, self.buoy.center_y.get), self.buoy, self.collision_validator, self.align_task, right=right, first_buoy=first_buoy, yellow=yellow) self.seen_frames_checker = ConsistencyCheck(3, 3, strict=True) self.collision_checker = ConsistencyCheck(2, 2, strict=True) self.last_percent_frame = 0 self.PERCENT_FRAME_THRESHOLD = 2.5 self.PERCENT_FRAME_DELTA_THRESHOLD = 10 self.TIMEOUT = 100
class Buoy(Task): """ Wrapper around the BuoyRam class that will specifically ram a red or green buoy """ def __init__(self, buoy, right=True, first_buoy=False, yellow=False, align_only=False, *args, **kwargs): super().__init__(*args, **kwargs) # Instantiate the BuoyRam task self.buoy = buoy self.heading_task = HeadingRestore() if align_only: self.align_task = AlignTarget( self.location_validator, LocateAdjacentBuoy(self.location_validator, right=right), (self.buoy.r_side_x.get, self.buoy.center_y.get), self.buoy, self.heading_task) self.ram_task = BuoyAlign( self.location_validator, (self.buoy.r_side_x.get, self.buoy.center_y.get), self.buoy, self.collision_validator, self.align_task, right=right, first_buoy=first_buoy, yellow=yellow) elif yellow: self.align_task = AlignTarget( self.location_validator, LocateAdjacentBuoy(self.location_validator, right=right), (self.buoy.r_side_x.get, self.buoy.bottom_y.get), self.buoy, self.heading_task) self.ram_task = BuoyRam( self.location_validator, (self.buoy.r_side_x.get, self.buoy.bottom_y.get), self.buoy, self.collision_validator, self.align_task, right=right, first_buoy=first_buoy, yellow=yellow) else: self.align_task = AlignTarget( self.location_validator, LocateAdjacentBuoy(self.location_validator, right=right), (self.buoy.r_side_x.get, self.buoy.center_y.get), self.buoy, self.heading_task) self.ram_task = BuoyRam( self.location_validator, (self.buoy.r_side_x.get, self.buoy.center_y.get), self.buoy, self.collision_validator, self.align_task, right=right, first_buoy=first_buoy, yellow=yellow) self.seen_frames_checker = ConsistencyCheck(3, 3, strict=True) self.collision_checker = ConsistencyCheck(2, 2, strict=True) self.last_percent_frame = 0 self.PERCENT_FRAME_THRESHOLD = 2.5 self.PERCENT_FRAME_DELTA_THRESHOLD = 10 self.TIMEOUT = 100 def on_first_run(self): self.logv("Starting {} task".format(self.__class__.__name__)) def on_run(self): # Perform BuoyRam task if self.this_run_time - self.first_run_time > self.TIMEOUT: self.finish() self.loge("Buoy ({}) timed out!".format(self.buoy)) return self.ram_task() if self.ram_task.finished: self.finish() def on_finish(self): self.logv("Buoy ({}) task finished in {} seconds!".format( self.buoy, self.this_run_time - self.first_run_time)) Zero()() def location_validator(self): # TODO even more robust location validator return self.seen_frames_checker.check(self.buoy.probability.get() != 0) def collision_validator(self): # TODO even more robust collision validator, susceptible to false # positives # if not shm.gpio.wall_1.get(): # self.logi("Detected buoy ram using touch sensor!") # return True current = self.buoy.percent_frame.get() # self.logv("Buoy Percent Frame : {}".format(current)) if current >= self.PERCENT_FRAME_THRESHOLD: # if self.collision_checker.check(abs(self.last_percent_frame - current) <= self.PERCENT_FRAME_DELTA_THRESHOLD): # self.logv("Returned true!") return True # self.last_percent_frame = current return False
class FindPinger(StatefulTask): def __init__(self, method, ontop_of_pinger=ontop_of_pinger_elevation, *args, **kwargs): super().__init__(*args, **kwargs) self.track_method = method self.ontop_of_pinger = ontop_of_pinger def on_first_run(self): assert isinstance(self.track_method, TrackMethod) self.follow_task = None self.heading_to_pinger = None set_gain() shm.hydrophones_settings.track_frequency_target.set(PINGER_FREQUENCY) shm.hydrophones_settings.track_magnitude_threshold.set( TRACK_MAG_THRESH) shm.hydrophones_settings.track_cooldown_samples.set( TRACK_COOLDOWN_SAMPLES) shm.navigation_settings.position_controls.set(1) shm.navigation_settings.optimize.set(0) self.localizer = Localizer(PINGER_FREQUENCY) self.has_made_progress = False self.hydro_watcher = shm.watchers.watcher() self.hydro_watcher.watch(shm.hydrophones_results_track) self.time_since_last_ping = self.this_run_time self.pinger_positions = collections.deque(maxlen=7) self.silencer = ThrusterSilencer() self.pinger = None self.ping_deviating_checker = ConsistencyCheck(7, 10) ontop_success = None if STOP_OVER_PINGER: ontop_success = 2 else: ontop_success = 6 self.ontop_checker = ConsistencyCheck(ontop_success, 5) self.pings = [] def generate_states(self): return "listen", { "listen": { "enter": self.enter_listen, "exit": self.exit_listen, "tick": self.listen }, "follow": { "enter": self.enter_follow, "tick": self.follow } } # Check for new pings, if there is one return its heading and elevation and add it to the # pings list def update_pings(self): self.silencer() # TODO Better way to filter out bad pings when thrusters are running. # If the watcher has not changed, there is no new ping if not self.hydro_watcher.has_changed(): # TODO Do something here if too much time has passed since last ping. return None # There is a new ping! self.time_since_last_ping = self.this_run_time # TODO Will this be long after the watcher fired? # Need to ensure that there is little delay. results = shm.hydrophones_results_track.get() kalman = shm.kalman.get() phases = (results.diff_phase_x, results.diff_phase_y) if not self.localizer.is_valid(phases[0], phases[1]): self.logi("Warning: Measured phases (%0.3f %0.3f) are not possible given " "physical parameters" % \ (phases[0], phases[1])) head, elev = self.localizer.get_heading_elevation(*phases) abs_head = (head + shm.kalman.heading.get()) % 360 self.logi("Ping: Phases: (%0.3f %0.3f), Relative Heading: %0.5f, Absolute " "Heading: %0.3f, Elevation: %0.1f" % \ (phases[0], phases[1], head, abs_head, elev)) in_silence = self.silencer.in_silence() # TODO Better way to detect ping period. this_ping_time = \ results.daemon_start_time + results.tracked_ping_time self.silencer.schedule_silence(this_ping_time + PINGER_PERIOD - 0.2, 3.0) self.logi("This time: %0.3f, This ping time %0.3f, Diff: %0.3f" % (self.this_run_time, this_ping_time, self.this_run_time - this_ping_time)) # If the thrusters are not in silence, then we may have heard thruster # noise and thought it was a ping; ignore it if not in_silence: self.logi("Heard ping but thrusters are not silenced, ignoring") return None # Record ping data sub_pos = get_sub_position(kalman) sub_quat = get_sub_quaternion(kalman) ping_data = PingData(phases, head, elev, sub_pos, sub_quat) self.pings.append(ping_data) return ping_data def enter_listen(self): self.logi("Stopping to listen for pings") kalman = shm.kalman.get() shm.navigation_settings.position_controls.set(1) desires = shm.navigation_desires.get() desires.north = kalman.north desires.east = kalman.east shm.navigation_desires.set(desires) # Clear previous pings self.pings = [] def exit_listen(self): self.logi("Exiting listen state") shm.navigation_settings.position_controls.set(0) def listen(self): self.update_pings() # If we haven't even gotten MIN_CONSISTENT_PINGS in total, there won't be # enough consistent ones, no point in continuing if len(self.pings) < MIN_CONSISTENT_PINGS: return # We want to make sure pings are consistent before following them. To do # this, first cluster the pings by heading headings, elevations = zip(*[ self.localizer.get_heading_elevation(*ping.phases) for ping in self.pings ]) data = get_clusterable(headings) clusters = fclusterdata(data, 8, criterion="distance") # The best cluster is the one with the most pings in it counted = collections.Counter(clusters) best_cluster, n_best = max(counted.items(), key=lambda item: item[1]) # To follow a heading, we require that at least three pings are in the # cluster which contains it if n_best >= MIN_CONSISTENT_PINGS: # Compute average phase for best cluster good_pings = [self.pings[i] for i, cluster_num in enumerate(clusters) \ if cluster_num == best_cluster] x_phases = [ping.phases[0] for ping in good_pings] y_phases = [ping.phases[1] for ping in good_pings] avg_phase_x = np.mean(x_phases) avg_phase_y = np.mean(y_phases) self.logi("Average phases (%f, %f) for best cluster %s" % \ (avg_phase_x, avg_phase_y, str(self.pings))) if self.track_method == TrackMethod.position: # Localize pinger to a position for ping in good_pings: self.localizer.add_observation(ping.phases, ping.sub_pos, ping.sub_quat) est_pinger_pos = self.localizer.compute_position() self.pinger_positions.append(est_pinger_pos) self.logi("Localized pinger to: %s" % str(est_pinger_pos)) self.logi("All estimated positions: %s" % str(self.pinger_positions)) self.follow_position = est_pinger_pos elif self.track_method == TrackMethod.heading: # Update the heading and elevation of the pinger pinger_head, elev = self.localizer.get_heading_elevation( avg_phase_x, avg_phase_y) sub_head = shm.kalman.heading.get() self.heading_to_pinger = heading = (pinger_head + sub_head) % 360 self.follow_elevation = elev return "follow" def elevation_to_distance(self, elevation): return HYDROPHONES_PINGER_DEPTH * math.tan(math.radians(elevation)) def enter_follow(self): self.logi("Following a heading of %0.3f" % self.heading_to_pinger) self.follow_change_heading = Heading() self.follow_inital_heading = Heading(self.heading_to_pinger + heading_offset) self.follow_vel_x = VelocityX() self.follow_vel_y = VelocityY() distance_to_pinger = self.elevation_to_distance(self.follow_elevation) self.follow_vel_x(x_dir * self.get_follow_speed(distance_to_pinger)) self.follow_vel_y(0.0) # Proportional control to slow down near the pinger def get_follow_speed(self, distance): # Start slowing down at SLOW_DOWN_DISTANCE speed = distance / SLOW_DOWN_DISTANCE # Don't do nothing dumb if speed > MAX_FOLLOW_SPEED: speed = MAX_FOLLOW_SPEED if speed < MIN_FOLLOW_SPEED: speed = MIN_FOLLOW_SPEED return speed def follow_heading(self, new_ping): distance_to_pinger = self.elevation_to_distance(new_ping.elevation) speed = self.get_follow_speed(distance_to_pinger) # Check if the ping suggests the pinger is in a heading deviating from # the current follow heading new_ping_heading = (new_ping.heading + shm.kalman.heading.get()) % 360 heading_deviation = abs(new_ping_heading - self.heading_to_pinger) # When we are close to the pinger, headings will vary more, allow for # deviation # Note, this is fairly broken, because it assumes we get good pings # while moving! Realistically, we should just move forward for a set time, # and then get new heading. BUT, since the consistency check will really # just wait for the requisite number of pings (since its a safe assumption # that we're just getting garbage), we can essentially turn this into # a makeshift timer by changing the window size on consistency check (:139) if new_ping.elevation > MIN_DEVIATING_PING_ELEVATION: deviating_ping = heading_deviation > MAX_FOLLOW_HEADING_DEVIATION if deviating_ping: self.logi( "Ping heading deviated from follow heading by %0.3f, more " "than maximum allowed" % heading_deviation) else: self.heading_to_pinger = new_ping_heading if self.ping_deviating_checker.check(deviating_ping): self.logi( "Consistently getting deviating pings! Stopping to listen." ) self.ping_deviating_checker.clear() return "listen" self.logi("Going straight for the pinger!") self.follow_vel_x(x_dir * speed) self.follow_vel_y(0.0) self.follow_change_heading(self.heading_to_pinger + heading_offset) else: velocity = rotate([speed, 0], new_ping.heading) self.logi("We are close! Translating to pinger: Velocity (%0.3f, " "%0.3f)" % (velocity[0], velocity[1])) self.follow_vel_x(velocity[0]) self.follow_vel_y(velocity[1]) return None def follow_position(self): return None def follow(self): # Turn to face the pinger if not self.follow_inital_heading.finished: self.follow_inital_heading() return new_ping = self.update_pings() if new_ping is not None: # Check whether we are on top of the pinger ontop_of_pinger = self.ontop_of_pinger(new_ping) if (ontop_of_pinger): self.logi( "Ontop of pinger validator returned true! Waiting for " "consitent positives to terminate following.") if self.ontop_checker.check(ontop_of_pinger): self.finish() if self.track_method == TrackMethod.heading: return self.follow_heading(new_ping) elif self.track_method == TrackMethod.position: return self.follow_position(new_ping)
class AlignTarget(Task): """ Aligns using ForwardTarget on a target coordinate, while ensuring that the target is visible """ def __init__(self, validator, locator_task, target_coords, vision_group, heading_task, forward_target_p=0.001, *args, **kwargs): """ validator - a function that returns True when the target is visible and False otherwise. locator_task - a task that locates the target target_coords - the coordinates of the target with which to align vision_group - the shm group for the buoy """ super().__init__(*args, **kwargs) self.validator = validator self.locator_task = locator_task self.heading_task = heading_task def get_center(): return (0, 0) # TODO use normalized coordinates instead self.target_task = ForwardTarget(target_coords, target=get_center, px=forward_target_p, dx=forward_target_p / 2, py=forward_target_p, dy=forward_target_p / 2, deadband=(30, 30)) self.target_checker = ConsistencyCheck(5, 5, strict=True) self.TIMEOUT = 60 # def on_first_run(self): # self.logv("Starting {} task".format(self.__class__.__name__)) def on_run(self): if self.this_run_time - self.first_run_time > self.TIMEOUT: self.finish() # self.loge("{} timed out!".format(self.__class__.__name__)) return # self.logv("Running {}".format(self.__class__.__name__)) # if shm.kalman.depth.get() < .4: # self.giveup_task() # self.finish() if self.validator(): # if abs(shm.kalman.depth.get() - BUOY_SEARCH_DEPTH) >= BUOY_DEPTH_VARIANCE: # self.depthless_target() # else: self.target_task() else: # self.heading_task() # HeadingRestore() self.logv('lost buoy? searching and restoring depth') Depth(constants.BUOY_SEARCH_DEPTH) self.locator_task() if self.target_checker.check(self.target_task.finished): self.finish()
def on_first_run(self, *args, **kwargs): self.logv("Checking if bins in sight") self.init_time = self.this_run_time self.seen_checker1 = ConsistencyCheck(6, 8) self.seen_checker2 = ConsistencyCheck(6, 8)
def on_first_run(self, task, success=18, total=20, *args, **kwargs): self.cons_check = ConsistencyCheck(success, total)
def __init__(self, seen_cons_check=(2, 3), unseen_cons_check=(5, 6)): self.last_obj = None self.tracking = False self.seen_cons_check = ConsistencyCheck(*seen_cons_check) self.unseen_cons_check = ConsistencyCheck(*unseen_cons_check)
class IdentifyBins(Task): """ Identifies which bin to drop markers into, centers over it """ def on_first_run(self, run_type, heading=None, uncovered_bin_vector=None): self.logi("Centering over bins...") self.logv("Starting IdentifyBins task") self.center_valid = False self.center_coords = (0, 0) self.task = DownwardTarget(px=0.0025, py=0.0025, point=lambda: self.center_coords, valid=lambda: self.center_valid) self.center_checker = ConsistencyCheck(15, 17) self.align_checker = ConsistencyCheck(6, 6) # TODO start alignment task. self.init_time = self.this_run_time self.uncovered_bin_vector = None self.seen_two = False cam = get_downward_camera() self.cover_tracker = Tracker(cam['width'] * 0.15) self.yellow_tracker = Tracker(cam['width'] * 0.15) def on_run(self, run_type, heading=None, uncovered_bin_vector=None): yellows = [TrackedBin(b.center_x, b.center_y) if b.probability > 0.0 else None for b in [yellow1.get(), yellow2.get()] ] cover_g = cover.get() covert = TrackedBin(cover_g.center_x, cover_g.center_y) if cover_g.probability > 0.0 else None self.consistent_bins = self.yellow_tracker.track(*yellows) self.consistent_cover = self.cover_tracker.track(covert, None) def calculate_bin_vector(bin1, bin2): body_frame = [(bin1.x, bin1.y), (bin2.x, bin2.y)] world_frame = [np.array(rotate(body, -shm.kalman.heading.get())) for body in body_frame] bin2bin = world_frame[1] - world_frame[0] return bin2bin / np.linalg.norm(bin2bin) if any(self.consistent_cover) and any(self.consistent_bins): if run_type == "cover": good_cover = self.consistent_cover[0] if good_cover is None: good_cover = self.consistent_cover[1] good_yellow = self.consistent_bins[0] if good_yellow is None: good_yellow = self.consistent_bins[1] bin2cover_hat = calculate_bin_vector(good_yellow, good_cover) if self.uncovered_bin_vector is None: # TODO Take average here. self.uncovered_bin_vector = bin2cover_hat self.logi("Discovered cover to bin world vector %0.2f %0.2f" % \ (self.uncovered_bin_vector[0], self.uncovered_bin_vector[1])) if run_type == "cover": cands = self.consistent_cover + self.consistent_bins else: if all(self.consistent_bins) and uncovered_bin_vector is not None: bin2bin = calculate_bin_vector(self.consistent_bins[0], self.consistent_bins[1]) if bin2bin.dot(uncovered_bin_vector()) > 0: index = 1 else: index = 0 if not self.seen_two: self.seen_two = True self.uncovered_ind = index self.logi("Chose bin with index %d: current coords %d %d" % \ (self.uncovered_ind, self.consistent_bins[self.uncovered_ind].x, self.consistent_bins[self.uncovered_ind].y)) else: if self.uncovered_ind == index: self.logv("Confirmed uncovered bin has index %d" % index) else: self.logi("WARNING: Detected new uncovered bin index %d!" % index) if not self.seen_two: self.logv("Did not find two yellow!") cands = self.consistent_bins + self.consistent_cover else: cands = [self.consistent_bins[self.uncovered_ind], self.consistent_bins[1 - self.uncovered_ind]] + self.consistent_cover for i, cand in enumerate(cands): if cand is not None: self.logv("Found good bin of index %d" % i) self.center_valid = True self.center_coords = cand.x, cand.y break else: self.logv("No good bins found to center on!") self.center_valid = False # Assumes cover and contours from same camera target = get_downward_camera_center() # TODO Increase deadband as time increases. self.task(target=target, deadband=(25, 25)) if self.center_checker.check(self.task.finished): if run_type == "cover" or heading is None: if heading is None: target_heading = shm.kalman.heading.get() + cover_g.angle else: target_heading = heading() align_task = Heading(target_heading, deadband=0.5) align_task() if self.align_checker.check(align_task.finished): VelocityX(0)() VelocityY(0)() self.finish() else: self.finish() else: self.align_checker.clear() def on_finish(self): self.logi("Centered!") self.logv('IdentifyBins task finished in {} seconds!'.format( self.this_run_time - self.init_time))
def on_first_run(self, task, success=18, total=20, *args, **kwargs): self.cons_check = ConsistencyCheck(success, total)
def on_first_run(self, vision): self.seen_cons_check = ConsistencyCheck(4, 5)
class ScuttleYellowBuoy(Task): """ Locates and scuttles a yellow buoy by dragging it down. Precondition: The yellow buoy is located at a position in front of the position of the submarine. Postcondition: The submarine will have dragged down the yellow buoy, and the submarine will be positioned above the yellow buoy. """ def __init__(self, *args, **kwargs): super().__init__(*args, **kwargs) self.locator_task = LocateBuoy(self.location_validator) self.locator_task = AssumeBuoy(self.location_validator) self.align_task = AlignTarget(self.location_validator, self.locator_task, (yellow_buoy_results.top_x.get, yellow_buoy_results.top_y.get), yellow_buoy_results, forward_target_p=0.002) self.concurrent_align_task = AlignTarget(self.location_validator, self.locator_task, (yellow_buoy_results.top_x.get, yellow_buoy_results.top_y.get), yellow_buoy_results, forward_target_p=0.002) self.MAX_RAM_SPEED = 0.6 self.ram_speed = self.MAX_RAM_SPEED self.ram_task = RamTarget(self.location_validator, self.collision_validator, self.locator_task, self.concurrent_align_task, ram_speed=lambda: self.ram_speed) self.ready_to_ram_checker = ConsistencyCheck(40, 40) self.seen_frames_checker = ConsistencyCheck(5, 5) SCUTTLE_TIME = 2.0 BUOY_MOUNTING_DISTANCE = 0.55 self.scuttle = Sequential(Log("Rising to prepare for scuttle"), RelativeToInitialDepth(-0.2, error=0.02), Log("Moving on top of Yellow Buoy"), MoveX(BUOY_MOUNTING_DISTANCE), Log("Beginning %0.2f second scuttle!" % SCUTTLE_TIME), MasterConcurrent(Timer(SCUTTLE_TIME), VelocityX(1.0), RelativeToInitialDepth(3.0, error=0.03)), Log("Scuttle complete. Returning to targeting position."), Depth(1.0) # GoToPosition(lambda: self.target_position[0], lambda: self.target_position[1], depth=lambda: self.target_depth) ) self.tasks = Sequential(self.locator_task, self.align_task, self.ram_task, self.scuttle) #self.last_percent_frame = 0 #self.PERCENT_FRAME_DELTA_THRESHOLD = 10 self.PERCENT_FRAME_THRESHOLD = 2 self.PERCENT_FRAME_SLOWDOWN_THRESHOLD = 0.4 self.TIMEOUT = 100 def on_first_run(self): self.logv("Starting {} task".format(self.__class__.__name__)) def on_run(self): # Locate the yellow buoy # Align with the yellow buoy # Move forward until collision with the buoy (using RamTarget) # Descend to drag buoy downwards if self.this_run_time - self.first_run_time > self.TIMEOUT: self.finish() self.loge("{} timed out!".format(self.__class__.__name__)) return self.tasks() if self.tasks.finished: self.finish() def on_finish(self): self.logv('{} task finished in {} seconds!'.format( self.__class__.__name__, self.this_run_time - self.first_run_time)) Zero()() def location_validator(self): # TODO even more robust location validator return self.seen_frames_checker.check(yellow_buoy_results.probability.get() != 0) def collision_validator(self): # TODO even more robust collision validator current = yellow_buoy_results.percent_frame.get() aligned = self.concurrent_align_task.finished max_ram_speed = self.MAX_RAM_SPEED if not aligned: max_ram_speed /= 3 self.ram_speed = scaled_speed(final_value=self.PERCENT_FRAME_THRESHOLD + 1, initial_value=self.PERCENT_FRAME_SLOWDOWN_THRESHOLD, final_speed=0.0, initial_speed=max_ram_speed, current_value=current) if self.ready_to_ram_checker.check(current >= self.PERCENT_FRAME_THRESHOLD and \ aligned): self.target_position = get_sub_position() self.target_depth = shm.kalman.depth.get() self.logi("Close enough to yellow buoy to scuttle!") return True #if yellow_buoy_results.center_y.get() > (shm.camera.forward_height.get() - 10) \ # and abs(self.last_percent_frame - current) <= self.PERCENT_FRAME_DELTA_THRESHOLD: # return True #self.last_percent_frame = current return False