class PathfindingService(object): PATHFINDING_NODE_COUNT = Vector2(60, 30) MAX_SEGMENT_LENGTH_CM = Vector2.uniform(50) DETECTION_WAIT_TIME = 10 @property def display_pathfinding_nodes(self): return display_pathfinding_nodes @display_pathfinding_nodes.setter def display_pathfinding_nodes(self, value): self._display_pathfinding_nodes = value @property def path_simplification_enabled(self): return self._pathfinder_config.simplify_path @path_simplification_enabled.setter def path_simplification_enabled(self, value): self._pathfinder_config.simplify_path = value @property def current_path(self): return self._current_path @property def current_target_location(self): return self._current_target_location def __init__(self, pathfinder, world_map_service, coordinate_factory): self._pathfinder_config = PathfinderConfig() self._coordinate_factory = coordinate_factory self._pathfinder = pathfinder self._world_map_service = world_map_service self._world_map = world_map_service.world_map self._treasure_path_selector = TreasurePathSelector(self._world_map) self._island_path_selector = IslandPathSelector(self._world_map) self._display_pathfinding_nodes = False self._current_path = None def reset(self): self._pathfinder.reset() def draw(self, image): self._pathfinder.draw(image, self._display_pathfinding_nodes) return image def find_path_to_charging_station(self): self._world_map_service.wait_for_detection( [WorldObjectType.ROBOT, WorldObjectType.CHARGING_STATION], strict=False, timeout=self.DETECTION_WAIT_TIME ) self._current_target_location = self._world_map.charging_station.get_location() docking_location = self._world_map.charging_station.get_docking_location(self._world_map.robot) node_path = self._find_path(docking_location) return self._create_path(node_path), self._current_target_location def find_path_to_island(self, island_descriptor): self._world_map_service.wait_for_detection( [WorldObjectType.ROBOT], strict=False, timeout=self.DETECTION_WAIT_TIME ) islands = self._world_map_service.get_islands(island_descriptor) best_island = self._find_best_island(islands) self._current_target_location = best_island.get_center() node_path = self._find_path(best_island.get_center()) return self._create_path(node_path), best_island def _find_best_island(self, islands): if len(islands) == 1: return islands[0] island_paths = [] for island in islands: try: island_paths.append((island, self._find_path(island.get_center()))) except NoPathFoundError: continue if len(island_paths) == 0: raise NoPathFoundError("No suitable path found for any of the qualifying islands.") return self._island_path_selector.select(island_paths)[0] def find_path_to_treasure(self, treasure_index=None): self._world_map_service.wait_for_detection( [WorldObjectType.ROBOT], strict=False, timeout=self.DETECTION_WAIT_TIME ) if treasure_index is not None: treasures = self._world_map_service.get_treasures()[treasure_index] else: treasures = self._world_map_service.get_pickable_treasures() best_treasure = self._find_best_treasure(CollectionUtils.get_iterable(treasures)) self._current_target_location = best_treasure.get_location() node_path = self._find_path(best_treasure.get_pickup_location(self._world_map.playfield, self._world_map.robot)) return self._create_path(node_path), best_treasure def _find_best_treasure(self, treasures): if len(treasures) == 1: return treasures[0] treasure_paths = [] for treasure in treasures: try: treasure_pickup_location = treasure.get_pickup_location( self._world_map.playfield, self._world_map.robot ) treasure_paths.append((treasure, self._find_path(treasure_pickup_location))) except NoPathFoundError: continue if len(treasure_paths) == 0: raise NoPathFoundError("No suitable path found for any of the detected treasures.") return self._treasure_path_selector.select(treasure_paths)[0] def _find_path(self, destination): self._init_pathfinder_config(destination) return self._pathfinder.find_path(self._pathfinder_config) def _init_pathfinder_config(self, destination): self._pathfinder_config.starting_point = self._world_map.robot.get_center() self._pathfinder_config.destination_point = destination self._pathfinder_config.grid_rect = self._world_map.playfield.rect self._pathfinder_config.grid_node_count = self.PATHFINDING_NODE_COUNT self._pathfinder_config.actor_size = self._world_map.robot.get_size().width self._pathfinder_config.max_segment_length = ( self._coordinate_factory.create(self.MAX_SEGMENT_LENGTH_CM, CoordinateSystem.PHYSICAL).get().length ) self._pathfinder_config.reference_image = self._world_map.reference_image self._pathfinder_config.obstacles = [] for island in self._world_map.islands: self._pathfinder_config.add_obstacle(island.get_bounding_circle()) def _create_path(self, node_path): self._current_path = node_path.drawable_path return self._current_path def _sort_treasures_by_distance(self, source_location, treasures): return sorted(treasures, key=lambda treasure: Segment(source_location, treasure.get_location()).length)