Python helpers.ConsistencyCheck类代码示例

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 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 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))

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 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 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 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, 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 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):
        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")

    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, 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 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))

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()