コード例 #1
0
 def update(self) -> None:
     position = self._vision_service.get_robot()
     robot_position = Position(position.coordinate.to_centimeters(),
                               position.orientation)
     data = robot_position.serialize()
     self._communication_service.send_message(
         Message("position", position=data))
コード例 #2
0
    def _find_destinations(self) -> None:
        source = self._vision_service.get_source()
        source = Position(source.coordinate.to_centimeters(), source.orientation)
        self.source_pos = self._approach_position_finder.calculate_from(source)

        goal = self._vision_service.get_goal()
        goal = Position(goal.coordinate.to_centimeters(), goal.orientation)
        self.goal_pos = self._approach_position_finder.calculate_from(goal)
コード例 #3
0
    def qr_code(self) -> Table:
        with self._populating:
            if not self._got_qr_code.is_set():
                self._set_layout()

                found_path = False
                for position in [
                        Position(Coord(190, 40), Angle(pi)),
                        Position(Coord(170, 60), Angle(7 * pi / 8))
                ]:
                    try:
                        self._qr_code = self._pathfinder.pathable_to(position)
                        found_path = True
                    except CannotPathThereError:
                        continue
                if not found_path:
                    raise CannotPathThereError

                self._got_qr_code.set()
        return self._qr_code
コード例 #4
0
    def charge_station(self) -> Table:
        with self._populating:
            if not self._got_charge_station.is_set():
                self._set_layout()

                charge_station = Position(Coord(20, 80), Angle(0))
                self._charge_station = self._pathfinder.pathable_to(
                    charge_station)

                self._got_charge_station.set()
        return self._charge_station
コード例 #5
0
    def home(self) -> Table:
        with self._populating:
            if not self._got_home.is_set():
                self._set_layout()

                home = Coord(55, 55)
                self._home = self._pathfinder.pathable_to(
                    Position(home, Angle(0)))

                self._got_home.set()
        return self._home
コード例 #6
0
    def get_robot(self) -> Position:
        if not self._initialized.is_set():
            raise VisionServiceNotInitialized

        self._robot, robot_orientation = self._robot_finder.find(self._image)

        robot_coord = self._camera_calibration.convert_robot_pixel_to_real(
            self._robot)
        logger.debug('[get_robot] x: {}, y: {}, orientation: {}'.format(
            robot_coord.x, robot_coord.y, robot_orientation))
        return Position(robot_coord, robot_orientation)
コード例 #7
0
    def get_goal(self) -> Position:
        if not self._initialized.is_set():
            raise VisionServiceNotInitialized

        self._goal, goal_orientation = self._goal_finder.find(self._image)

        goal_coord = self._camera_calibration.convert_table_pixel_to_real(
            self._goal.get_center())
        logger.info('[get_goal] x: {}, y: {}, orientation: {}'.format(
            goal_coord.x, goal_coord.y, goal_orientation))
        return Position(goal_coord, goal_orientation)
コード例 #8
0
 def update(self) -> None:
     new_image = self._vision_service.get_image()
     new_path = self._path_service.get_path()
     image_with_path = self._draw_on(new_image, new_path)
     robot_position = self._vision_service.get_robot()
     robot_position_in_centimeters = Position(
         robot_position.coordinate.to_centimeters(),
         robot_position.orientation)
     self._current_path_image = self._draw_robot_on(
         image_with_path, robot_position_in_centimeters)
     self._notify()
コード例 #9
0
    def source(self) -> Table:
        with self._populating:
            if not self._got_source.is_set():
                self._set_layout()

                for i in range(3):
                    try:
                        position = self._vision_service.get_source()
                        break
                    except SourceCouldNotBeFound:
                        logger.info("SourceCouldNotBeFound")
                source = Position(position.coordinate.to_centimeters(),
                                  position.orientation)
                approach_position = self._approach_position_finder.calculate_from(
                    source)
                self._source = self._pathfinder.pathable_to(approach_position)

                self._got_source.set()
        return self._source
コード例 #10
0
    def _get_adjacent_cells(self, cell: Coord, table: Table) -> List[Coord]:
        possible_cells = [
            Coord(cell.x, cell.y - 1),
            Coord(cell.x + 1, cell.y - 1),
            Coord(cell.x + 1, cell.y),
            Coord(cell.x + 1, cell.y + 1),
            Coord(cell.x, cell.y + 1),
            Coord(cell.x - 1, cell.y + 1),
            Coord(cell.x - 1, cell.y),
            Coord(cell.x - 1, cell.y - 1)
        ]

        valid_cells = []
        for possible_cell in possible_cells:
            try:
                if table[possible_cell] < 0:
                    valid_cells.append(possible_cell)
            except KeyError:
                pass

        return valid_cells


if __name__ == '__main__':
    template_table = Table(30, 30, -1, 7, Angle(0), 1)
    obstacles = [Coord(14, 14)]
    grassfire_pathfinder = GrassfirePathfinder(template_table, obstacles)
    dest_map = grassfire_pathfinder.pathable_to(Position(Coord(20, 20), Angle(0)))
    print(dest_map)
コード例 #11
0
class AdaptivePathableCatalog(IPathableCatalog):
    charge_position_pos = Position(Coord(21, 80), Angle(0))
    qr_codes_pos = [Position(Coord(190, 40), Angle(pi)), Position(Coord(170, 60), Angle(7 * pi / 8))]
    home_pos = Position(Coord(55, 55), Angle(0))

    def __init__(self, vision_service: VisionService, pathfinder_factory: IPathfinderFactory,
                 approach_position_finder: IApproachPositionFinder) -> None:
        self._vision_service = vision_service
        self._pathfinder_factory = pathfinder_factory
        self._approach_position_finder = approach_position_finder

        self._populating = Lock()
        self._filled = Event()

    @property
    def home(self) -> Table:
        with self._populating:
            if not self._filled.is_set():
                self._fill()
            return self._home

    @property
    def charge_station(self) -> Table:
        with self._populating:
            if not self._filled.is_set():
                self._fill()
            return self.charge_station

    @property
    def qr_code(self) -> Table:
        with self._populating:
            if not self._filled.is_set():
                self._fill()
            return self._qr_code

    @property
    def goal(self) -> Table:
        with self._populating:
            if not self._filled.is_set():
                self._fill()
            return self._goal

    @property
    def source(self) -> Table:
        with self._populating:
            if not self._filled.is_set():
                self._fill()
            return self._source

    def _fill(self) -> None:
        obstacles = self._find_obstacles()
        self._find_destinations()

        pathfinder = self._pathfinder_factory.create(obstacles)
        while not self._filled.is_set():
            self._charge_station: Table = pathfinder.pathable_to(self.charge_position_pos)
            found_path = False
            for qr_code in self.qr_codes_pos:
                try:
                    self._qr_code = pathfinder.pathable_to(qr_code)
                    found_path = True
                except CannotPathThereError:
                    continue
            if not found_path:
                raise CannotPathThereError
            self._home: Table = pathfinder.pathable_to(self.home_pos)
            self._source: Table = pathfinder.pathable_to(self.source_pos)
            self._goal: Table = pathfinder.pathable_to(self.goal_pos)
            try:
                assert self._charge_station[self.home_pos.coordinate] != -1 and self._charge_station[
                    self.home_pos.coordinate] != float("inf")
                assert self._qr_code[self.home_pos.coordinate] != -1 and self._qr_code[
                    self.home_pos.coordinate] != float("inf")
                assert self._source[self.home_pos.coordinate] != -1 and self._source[
                    self.home_pos.coordinate] != float("inf")
                assert self._goal[self.home_pos.coordinate] != -1 and self._goal[
                    self.home_pos.coordinate] != float("inf")
                self._filled.set()
            except AssertionError:
                self._pathfinder_factory.exclusion_radius -= 1
                print(self._pathfinder_factory.exclusion_radius)
                pathfinder = self._pathfinder_factory.create(obstacles)

    def _find_obstacles(self) -> List[Coord]:
        found_obstacles = False
        count = 3
        obstacles = []
        while not found_obstacles and count > 0:
            try:
                self._vision_service.update()
                obstacles = [obstacle.to_centimeters() for obstacle in self._vision_service.get_obstacles()]
                found_obstacles = True
            except VisionError:
                count -= 1
        return obstacles

    def _find_destinations(self) -> None:
        source = self._vision_service.get_source()
        source = Position(source.coordinate.to_centimeters(), source.orientation)
        self.source_pos = self._approach_position_finder.calculate_from(source)

        goal = self._vision_service.get_goal()
        goal = Position(goal.coordinate.to_centimeters(), goal.orientation)
        self.goal_pos = self._approach_position_finder.calculate_from(goal)
コード例 #12
0
 def test_when_pathing_to_some_point_then_gives_the_right_map(self) -> None:
     path = self.grassfire_pathfinder.pathable_to(Position(Coord(24, 24), Angle(0)))
     self.assertEqual(path.data, EXPECTED_PATH)