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