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)