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