Python ConsistencyCheck.clear方法代码示例

# 需要导入模块: from mission.framework.helpers import ConsistencyCheck [as 别名]
# 或者: from mission.framework.helpers.ConsistencyCheck import clear [as 别名]
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))

# 需要导入模块: from mission.framework.helpers import ConsistencyCheck [as 别名]
# 或者: from mission.framework.helpers.ConsistencyCheck import clear [as 别名]
class ConsistentObject:
    """
    Consistency-check an object's existence

    Inputted objects must have an x and y field
    """
    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

# 需要导入模块: from mission.framework.helpers import ConsistencyCheck [as 别名]
# 或者: from mission.framework.helpers.ConsistencyCheck import clear [as 别名]

#.........这里部分代码省略.........
        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))