class TestPathfinder(object): GRID_NODE_COUNT = Vector2(10, 10) GRID_RECT = Rect.from_size(Vector2(10, 10)) SAMPLE_ACTOR_SIZE = 1 @mock.patch('services.pathfinding.pathfinder.Pathfinder.PathSimplifier', autospec = False) def setUp(self, path_simplifier_mock): self.pathfinder = Pathfinder(PathSimplifier()) self._setup_pathfinder_config() def _setup_pathfinder_config(self): self._pathfinder_config = PathfinderConfig() self._pathfinder_config.starting_point = Vector2(0, 0) self._pathfinder_config.destination_point = Vector2(self.GRID_RECT.width, self.GRID_RECT.height) self._pathfinder_config.grid_rect = self.GRID_RECT self._pathfinder_config.grid_node_count = self.GRID_NODE_COUNT self._pathfinder_config.actor_size = self.SAMPLE_ACTOR_SIZE def test_given_no_obstacles_when_finding_path_then_simplified_path_is_found(self): node_path = self.pathfinder.find_path(self._pathfinder_config) assert_equals(node_path.node_count, 2) def test_given_an_obstacle_when_finding_path_then_simplified_path_is_found(self): self._pathfinder_config.add_obstacle(Circle(self.GRID_RECT.center, 2)) node_path = self.pathfinder.find_path(self._pathfinder_config) assert_equals(node_path.node_count, 4)
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)
class TestPathfinder(object): GRID_NODE_COUNT = Vector2(10, 10) GRID_RECT = Rect.from_size(Vector2(10, 10)) SAMPLE_ACTOR_SIZE = 1 @mock.patch("services.pathfinding.pathfinder.PathSimplifier.PathSimplifier", autospec=False) def setup(self, path_simplifier_mock): path_simplifier_mock_instance = path_simplifier_mock.return_value path_simplifier_mock_instance.simplify_path.side_effect = self.simplify_path_mock self._pathfinder = Pathfinder(path_simplifier_mock_instance) self._setup_pathfinder_config() def _setup_pathfinder_config(self): self._pathfinder_config = PathfinderConfig() self._pathfinder_config.starting_point = Vector2(0, 0) self._pathfinder_config.grid_rect = self.GRID_RECT self._pathfinder_config.grid_node_count = self.GRID_NODE_COUNT self._pathfinder_config.actor_size = self.SAMPLE_ACTOR_SIZE def simplify_path_mock(self, node_path, grid_graph, max_segment_length): return node_path def test_given_no_obstacles_when_finding_path_then_path_is_found(self): self._pathfinder_config.destination_point = Vector2(self.GRID_RECT.width, self.GRID_RECT.height) node_path = self._pathfinder.find_path(self._pathfinder_config) assert_true(node_path.node_count > 0) def test_given_destination_point_same_as_starting_point_when_finding_path_then_path_is_found_with_single_vertex( self ): self._pathfinder_config.destination_point = Vector2(0, 0) node_path = self._pathfinder.find_path(self._pathfinder_config) assert_equals(node_path.node_count, 1) def test_given_destination_point_beside_starting_point_when_finding_path_then_path_is_found_with_single_segment( self ): self._pathfinder_config.destination_point = Vector2(2, 2) node_path = self._pathfinder.find_path(self._pathfinder_config) assert_equals(node_path.node_count, 2) def test_given_destination_point_out_of_grid_when_finding_path_then_path_is_found(self): self._pathfinder_config.destination_point = Vector2(self.GRID_RECT.width + 1, self.GRID_RECT.height + 1) node_path = self._pathfinder.find_path(self._pathfinder_config) assert_true(node_path.node_count > 0) def test_given_obstacle_when_finding_path_then_path_is_found(self): self._pathfinder_config.destination_point = Vector2(self.GRID_RECT.width, self.GRID_RECT.height) self._pathfinder_config.add_obstacle(Circle(self.GRID_RECT.center, 2)) node_path = self._pathfinder.find_path(self._pathfinder_config) assert_true(node_path.node_count > 0) def test_given_destination_inside_obstacle_when_finding_path_then_path_is_found(self): self._pathfinder_config.destination_point = Vector2(self.GRID_RECT.width, self.GRID_RECT.height) self._pathfinder_config.add_obstacle(Circle(self._pathfinder_config.destination_point, 2)) node_path = self._pathfinder.find_path(self._pathfinder_config) assert_true(node_path.node_count > 0) @raises(NoPathFoundError) def test_given_no_possible_path_when_finding_path_then_error_is_raised(self): self._pathfinder_config.destination_point = Vector2(self.GRID_RECT.width, self.GRID_RECT.height) self._pathfinder_config.add_obstacle(Circle(Vector2(self.GRID_RECT.width / 2, 2), 4)) self._pathfinder_config.add_obstacle(Circle(Vector2(self.GRID_RECT.width / 2, 7), 4)) node_path = self._pathfinder.find_path(self._pathfinder_config) def test_given_path_found_when_drawing_path_then_path_is_successfully_drawn(self): self._pathfinder_config.destination_point = Vector2(self.GRID_RECT.width, self.GRID_RECT.height) node_path = self._pathfinder.find_path(self._pathfinder_config) blank_image = Image.from_attributes(self.GRID_RECT.width, self.GRID_RECT.height, ColorMode.BGR) self._pathfinder.draw(blank_image)