def test_ne_instance(self) -> None:
map1: SparseMap = SparseMap(Size(200, 200),
Agent(Point(20, 20), 10),
[Obstacle(Point(40, 40), 10), Obstacle(Point(100, 100), 40)],
Goal(Point(180, 160), 10))
map2: int = 2
self.assertNotEqual(map1, map2)
def test_convert_to_dense_map(self) -> None:
map1: SparseMap = SparseMap(
Size(4, 3),
Agent(Point(0, 1)),
[Obstacle(Point(0, 0)), Obstacle(Point(1, 0)), Obstacle(Point(2, 0)), Obstacle(Point(3, 0))],
Goal(Point(3, 2))
)
map2: DenseMap = map1.convert_to_dense_map()
self.assertEqual(map1, map2)
def test_ne_dense(self) -> None:
map1: SparseMap = SparseMap(Size(200, 200),
Agent(Point(20, 20), 10),
[Obstacle(Point(40, 40), 10), Obstacle(Point(100, 100), 40)],
Goal(Point(180, 160), 10))
map2: DenseMap = SparseMap(Size(200, 200),
Agent(Point(20, 20), 10),
[Obstacle(Point(40, 40), 10), Obstacle(Point(100, 100), 40)],
Goal(Point(180, 160), 10)).convert_to_dense_map()
self.assertNotEqual(map1, map2)
def test_eq_sparse_map(self) -> None:
map1: DenseMap = DenseMap(
[[[DenseMap.WALL_ID, DenseMap.WALL_ID, DenseMap.CLEAR_ID],
[DenseMap.AGENT_ID, DenseMap.CLEAR_ID, DenseMap.CLEAR_ID]],
[[DenseMap.CLEAR_ID, DenseMap.CLEAR_ID, DenseMap.CLEAR_ID],
[DenseMap.GOAL_ID, DenseMap.CLEAR_ID, DenseMap.CLEAR_ID]]])
map2: SparseMap = SparseMap(Size(3, 2, 2), Agent(Point(
0, 1, 0)), [Obstacle(Point(0, 0, 0)),
Obstacle(Point(1, 0, 0))], Goal(Point(0, 1, 1)))
self.assertEqual(map1, map2)
def test_ne_all(self) -> None:
map1: SparseMap = SparseMap(Size(200, 200, 200),
Agent(Point(20, 20, 20), 10), [
Obstacle(Point(40, 40, 40), 10),
Obstacle(Point(100, 100, 100), 40)
], Goal(Point(180, 160, 120), 10))
map2: SparseMap = SparseMap(Size(100, 200, 200),
Agent(Point(15, 20, 20), 10), [
Obstacle(Point(40, 30, 40), 10),
Obstacle(Point(100, 90, 100), 40)
], Goal(Point(180, 150, 120), 10))
self.assertNotEqual(map1, map2)
def set_grid(self, grid: List[List[int]]) -> None:
self.grid = grid
self.size = Size(len(grid[0]), len(grid))
for i in range(len(self.grid)):
for j in range(len(self.grid[i])):
if self.grid[i][j] == self.AGENT_ID:
self.agent = Agent(Point(j, i))
if self.grid[i][j] == self.GOAL_ID:
self.goal = Goal(Point(j, i))
if self.grid[i][j] == self.WALL_ID:
self.obstacles.append(Obstacle(Point(j, i)))
if self.grid[i][j] == self.EXTENDED_WALL:
self.obstacles.append(ExtendedWall(Point(j, i)))
def test_str_debug_level_3(self) -> None:
services: Services = Mock()
services.settings.simulator_write_debug_level = DebugLevel.HIGH
map1: SparseMap = SparseMap(
Size(30, 30),
Agent(Point(1, 2), 1),
[Obstacle(Point(5, 5), 100)],
Goal(Point(4, 3), 1),
services
).convert_to_dense_map()
self.assertEqual("""DenseMap: {
size: Size(30, 30),
agent: Agent: {position: Point(1, 2), radius: 1},
goal: Goal: {position: Point(4, 3), radius: 1},
obstacles: 898,
grid: [
1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
1, 2, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
1, 1, 1, 1, 3, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
]
}""", str(map1))
def __new_grid(self,
weight_grid: np.ndarray,
weight_bounds: Optional[Tuple[Real, Real]] = None,
traversable_threshold: Optional[Real] = None,
unmapped_value: Optional[Real] = None) -> None:
self.size = Size(*weight_grid.shape)
self.grid = np.full(self.size, self.CLEAR_ID, dtype=np.uint8)
self.weight_grid = np.empty(self.size, dtype=np.float32)
# bounds
if weight_bounds is None:
ignored = [] if unmapped_value is None else [unmapped_value]
weight_bounds = (min(flatten(weight_grid, ignored)),
max(flatten(weight_grid, ignored)))
# threshold
if traversable_threshold is None:
traversable_threshold = min(
weight_bounds[0] + (weight_bounds[1] - weight_bounds[0]) *
self.DEFAULT_TRAVERSABLE_THRESHOLD, weight_bounds[1])
self.traversable_threshold = OccupancyGridMap.__normalise(
weight_bounds, traversable_threshold)
# unmapped value
if unmapped_value is not None:
for idx in np.ndindex(*self.size):
if weight_grid[idx] == unmapped_value:
self.grid[idx] = self.UNMAPPED_ID
# obstacles
self.obstacles.clear()
for idx in np.ndindex(*self.size):
v = weight_grid[idx]
self.weight_grid[idx] = OccupancyGridMap.__normalise(
weight_bounds, v)
if v > traversable_threshold and self.grid[idx] != self.UNMAPPED_ID:
self.grid[idx] = self.WALL_ID
self.obstacles.append(Obstacle(Point(*idx)))
# entities
self.grid[self.agent.position.values] = self.AGENT_ID
self.grid[self.goal.position.values] = self.GOAL_ID
# todo: the following works but very slow. Furthermore, should
# implement for mutable maps. Disabled here when mutable because
# self.__update_grid() doesn't handle extended walls.
if not self.mutable:
self.extend_walls()
def set_grid(self, grid: np.array, transpose) -> None:
# We transpose here to not worry about flipping coordinates later on
# Please take care I'm not sure why everythng works but it does atm
self.grid = np.transpose(grid) if transpose else grid
self.size = Size(*self.grid.shape)
for index in np.ndindex(*self.size):
val: int = self.grid[index]
if val == self.AGENT_ID:
self.agent = Agent(Point(*index))
elif val == self.GOAL_ID:
self.goal = Goal(Point(*index))
elif val == self.WALL_ID:
self.obstacles.append(Obstacle(Point(*index)))
elif val == self.EXTENDED_WALL_ID:
self.obstacles.append(ExtendedWall(Point(*index)))
def __update_grid(self, occ_grid):
self.obstacles.clear()
for i in range(len(occ_grid)):
for j in range(len(occ_grid[i])):
pos = Point(j, i)
if self.agent.position != pos and self.goal.position != pos and occ_grid[
i][j] == self.WALL_ID:
self.obstacles.append(Obstacle(Point(j, i)))
if occ_grid[i][j] == self.EXTENDED_WALL:
occ_grid[i][j] = self.CLEAR_ID
occ_grid[self.agent.position.y][self.agent.position.x] = self.AGENT_ID
occ_grid[self.goal.position.y][self.goal.position.x] = self.GOAL_ID
self.grid = occ_grid
self.extend_walls()
def _static_init_(cls):
cls.pixel_map_small_obstacles: SparseMap = \
SparseMap(Size(100, 100),
Agent(Point(20, 60), 5),
[
Obstacle(Point(20, 50), 5),
Obstacle(Point(60, 40), 5),
Obstacle(Point(40, 80), 5),
Obstacle(Point(30, 10), 5),
Obstacle(Point(80, 80), 5),
Obstacle(Point(20, 40), 5),
],
Goal(Point(95, 95), 5))
cls.pixel_map_empty: SparseMap = \
SparseMap(Size(500, 500),
Agent(Point(245, 245), 10),
[],
Goal(Point(0, 0), 5))
cls.pixel_map_one_obstacle: SparseMap = \
SparseMap(Size(500, 500),
Agent(Point(40, 40), 10),
[
Obstacle(Point(250, 250), 50),
],
Goal(Point(460, 460), 10))
cls.pixel_map_one_obstacle_3d: SparseMap = \
SparseMap(Size(500, 500, 100),
Agent(Point(40, 40, 8), 10),
[
Obstacle(Point(250, 250, 50), 50),
],
Goal(Point(460, 460, 92), 10))
cls.grid_map_one_obstacle: SparseMap = \
SparseMap(Size(33, 32),
Agent(Point(0, 15)), # Point(x,y) x starts from 0 from left side (column); y starts from 0 from top (row)
[
Obstacle(Point(15, 13), 5),
],
Goal(Point(31, 15)))
cls.grid_map_one_obstacle1 = cls.grid_map_one_obstacle.convert_to_dense_map() # DenseMap shows the obstacles as black, SparseMap shows radius of obstacle as white
cls.grid_map_one_obstacle1.name = "Long Wall"
cls.grid_map_3d_no_obstacles: DenseMap = DenseMap([[[2, 0], [0, 0]], [[0, 0], [3, 0]]])
cls.grid_map_3d_one_obstacle: DenseMap = DenseMap([[[2, 0], [0, 0]], [[0, 1], [3, 0]]])
cls.grid_map_3d_complex_with_obstacles: DenseMap = DenseMap([[[0, 0, 0], [0, 1, 0], [2, 0, 0]], [[0, 1, 0], [0, 1, 0], [1, 0, 0]], [[0, 0, 3], [0, 0, 0], [0, 1, 0]]])
cls.grid_map_3d_example_3_3_3: DenseMap = DenseMap([[[0, 0, 0], [0, 0, 0], [2, 0, 0]], [[0, 0, 0], [0, 1, 0], [0, 0, 0]], [[0, 0, 3], [0, 0, 0], [0, 0, 0]]])
cls.grid_map_3d_example_4_4_3: DenseMap = DenseMap([[[0, 0, 0, 1], [1, 0, 0, 0], [2, 0, 1, 0], [0, 0, 0, 0]], [[0, 0, 0, 1], [0, 0, 0, 0], [
0, 1, 0, 0], [0, 0, 0, 1]], [[0, 0, 0, 0], [0, 0, 0, 3], [0, 0, 0, 0], [0, 0, 0, 1]]])
cls.grid_map_3d_example_4_4_4: DenseMap = DenseMap([[[0, 0, 0, 1], [1, 0, 0, 0], [0, 0, 1, 0], [0, 0, 0, 0]], [[0, 0, 0, 1], [1, 0, 0, 0], [2, 0, 1, 0], [0, 0, 0, 0]], [
[0, 0, 0, 1], [0, 0, 0, 0], [0, 1, 0, 0], [0, 0, 0, 1]], [[0, 0, 0, 0], [0, 0, 0, 3], [0, 0, 0, 0], [0, 0, 0, 1]]])
cls.large_map_one_obstacle_3d: SparseMap = \
SparseMap(Size(20, 20, 20),
Agent(Point(19, 4, 8), 1),
[
Obstacle(Point(15, 15, 5), 3),
],
Goal(Point(4, 19, 9), 1))
cls.grid_map_3d_example: DenseMap = cls.large_map_one_obstacle_3d.convert_to_dense_map()
cls.grid_map_3d_example.name = "3D Cube"
cls.grid_map_labyrinth: DenseMap = DenseMap([
[2, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 1, 3, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 1, 0, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
], name="Labyrinth")
cls.grid_map_labyrinth2: DenseMap = DenseMap([
# 0 is free space; 1 is obstacle; 2 is starting location; 3 is goal location
[2, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 3, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1, 1, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
])
cls.grid_map_complex_obstacle: DenseMap = DenseMap([
[2, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 1, 1, 1, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1, 1, 0, 0, 0, 0],
[0, 0, 1, 1, 0, 0, 0, 1, 1, 1, 1, 0, 0, 0, 0, 0],
[0, 0, 0, 1, 0, 0, 0, 1, 1, 1, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 1, 0, 0, 0, 1, 1, 1, 1, 1, 0, 0, 0, 0],
[0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0],
[0, 0, 0, 0, 0, 0, 1, 1, 1, 0, 0, 0, 3, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
], name="vin test 16x16 -1")
cls.grid_map_complex_obstacle2: DenseMap = DenseMap([
[2, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 1, 0, 0, 1, 1, 1, 0, 0, 1, 1, 0, 0, 0],
[0, 0, 0, 1, 0, 0, 0, 0, 1, 1, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 0, 0],
[0, 0, 1, 0, 1, 0, 0, 0, 0, 0, 1, 1, 1, 1, 0, 0],
[0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 1, 1, 1, 0, 0, 0],
[0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 1, 1, 0, 0, 0, 0],
[0, 0, 1, 1, 0, 0, 0, 1, 1, 1, 1, 0, 0, 0, 0, 0],
[0, 0, 0, 1, 0, 0, 0, 1, 1, 1, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 1, 0, 1, 0, 1, 1, 1, 1, 1, 0, 0, 0, 0],
[0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0],
[0, 1, 0, 0, 0, 0, 1, 1, 1, 0, 0, 0, 3, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
], name="vin test 16x16 -2")
cls.grid_map_small_one_obstacle: DenseMap = DenseMap([
[0, 0, 0, 0, 0, 0, 0, 0],
[0, 2, 0, 0, 0, 1, 1, 0],
[0, 0, 1, 0, 0, 1, 0, 0],
[1, 0, 1, 0, 0, 0, 0, 0],
[0, 0, 1, 1, 1, 1, 0, 0],
[0, 0, 1, 0, 1, 1, 0, 0],
[1, 0, 1, 0, 0, 1, 3, 0],
[0, 0, 0, 0, 0, 1, 1, 0],
], name="vin test 8x8 -2")
cls.grid_map_small_one_obstacle2: DenseMap = DenseMap([
[1, 1, 1, 1, 1, 1, 1, 1],
[1, 0, 0, 0, 0, 0, 0, 1],
[1, 0, 0, 0, 0, 0, 0, 1],
[1, 0, 0, 0, 0, 0, 0, 1],
[1, 0, 1, 1, 1, 1, 2, 1],
[1, 0, 1, 1, 1, 1, 0, 1],
[3, 0, 0, 0, 0, 0, 0, 1],
[1, 1, 1, 1, 1, 1, 1, 1],
], name="vin test 8x8")
cls.grid_map_small_one_obstacle3: DenseMap = DenseMap([
[2, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 1, 1, 0, 0, 0, 0],
[0, 0, 1, 0, 0, 0, 0, 0],
[0, 0, 0, 1, 1, 1, 1, 1],
[0, 0, 0, 1, 1, 1, 1, 1],
[0, 1, 0, 0, 0, 0, 0, 0],
[0, 0, 1, 0, 1, 1, 0, 0],
[3, 0, 0, 0, 0, 0, 0, 0],
], name="vin test 8x8 -3")
cls.grid_map_no_solution: DenseMap = DenseMap([
[0, 0, 0, 0, 0, 0, 0, 0],
[0, 1, 1, 1, 1, 1, 1, 0],
[0, 1, 0, 1, 2, 0, 1, 0],
[0, 1, 0, 1, 1, 0, 1, 0],
[0, 1, 0, 1, 1, 0, 1, 0],
[0, 1, 0, 0, 0, 0, 1, 0],
[0, 1, 1, 1, 1, 1, 1, 0],
[0, 0, 0, 0, 0, 0, 0, 3],
])
cls.grid_yt: DenseMap = DenseMap([
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 3],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0],
[0, 0, 0, 0, 0, 0, 0, 2, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
])
cls.grid_map_28x28: SparseMap = \
SparseMap(Size(28, 28),
Agent(Point(0, 0)), # Point(x,y) x starts from 0 from left side (column); y starts from 0 from top (row)
[
Obstacle(Point(15, 13), 5),
Obstacle(Point(21, 22), 2),
Obstacle(Point(6, 5), 3),
],
Goal(Point(28, 28)))
cls.ogm_2d_immutable = OccupancyGridMap([
[-1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1],
[-1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1],
[-1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1],
[-1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1],
[-1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1],
[-1, -1, -1, -1, -1, 1, 1, 1, 1, 1, 1, 1, 1, -1, -1, -1, -1, -1],
[-1, -1, -1, -1, -1, 1, 0, 0.2, 0.2, 0.3, 0, 0, 1, -1, -1, -1, -1, -1],
[-1, -1, -1, -1, -1, 1, 0, 0.5, 0, 0, 0, 0, 1, -1, -1, -1, -1, -1],
[-1, -1, -1, -1, -1, 1, 0, 0.7, 0.3, 0, 0, 0, 1, -1, -1, -1, -1, -1],
[-1, -1, -1, -1, -1, 1, 0, 1, 1, 1, 1, 0, 1, -1, -1, -1, -1, -1],
[-1, -1, -1, -1, -1, 1, 0, 1, 1, 1, 1, 0, 1, -1, -1, -1, -1, -1],
[-1, -1, -1, -1, -1, 0, 0, 0, 0, 0, 0, 0, 1, -1, -1, -1, -1, -1],
[-1, -1, -1, -1, -1, 1, 1, 1, 1, 1, 1, 1, 1, -1, -1, -1, -1, -1],
[-1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1],
[-1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1],
[-1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1],
[-1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1],
[-1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1]],
Agent(Point(7, 7), radius=1), Goal(Point(7, 9)), unmapped_value=-1, mutable=False)
cls.ogm_2d = OccupancyGridMap([ # used for dynamic growth debugging
[0, 0, 0.2, 0.3, 0.4, 0.8, 1, 1, 0.8, 0, 0, 1, 1, 0, 0, 0, 0, 0.9],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.5]] +
[[-1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1] for _ in range(40)],
Agent(Point(0, 0)), Goal(Point(0, 16)), unmapped_value=-1, name="Occupancy Grid 2D")
cls.ogm_3d_immutable = OccupancyGridMap([[[0, 0, 0.25, 1, 0, 0, 0.25, 1, 0, 0, 0.25, 1, 0, 0, 0.25, 1],
[1, 0, 0.56, 0, 1, 0, 0.56, 0, 1, 0, 0.56, 0, 1, 0, 0.56, 0],
[0.8, 0, 0, 0, 0.8, 0, 0, 0, 0.8, 0, 0, 0, 0.8, 0, 0, 0],
[0.2, 0.3, 0.4, 0.5, 0.2, 0.3, 0.4, 0.5, 0.2, 0.3, 0.4, 0.5, 0.2, 0.3, 0.4, 0.5]],
[[0.2, 0.4, 0.5, 0, 0.2, 0.4, 0.5, 0, 0.2, 0.4, 0.5, 0, 0.2, 0.4, 0.5, 0],
[0, 0, 0.2, 0, 0, 0, 0.2, 0, 0, 0, 0.2, 0, 0, 0, 0.2, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0.8, 0, 0, 0, 0.8, 0, 0, 0, 0.8, 0, 0, 0, 0.8, 0, 0]],
[[0.8, 0.8, 0, 0, 0.8, 0.8, 0, 0, 0.8, 0.8, 0, 0, 0.8, 0.8, 0, 0],
[0, 0, 0.0, 0, 0, 0, 0.0, 0, 0, 0, 0.1, 0, 0, 0, 0.1, 0],
[0, 1, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0],
[0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 1]],
[[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0.3908, 0, 0, 0.121, 0, 0, 0.213123, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.56, 0.23, 0.567, 0.9, 0],
[0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 1]],
[[0, 0, 0, 0, 0, 0, 0, 0, 0.56, 0.23, 0.567, 0.9, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0.245, 0, 0, 0, 0.56, 0.23, 0.567, 0.9, 0, 0, 0],
[0, 0, 0.56, 0.23, 0.567, 0.9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 1]],
[[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0.56, 0.23, 0.567, 0.9, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 1]],
[[0, 0, 0.25, 1, 0, 0, 0.25, 1, 0, 0, 0.25, 1, 0, 0, 0.25, 1],
[1, 0, 0.56, 0, 1, 0, 0.56, 0, 1, 0, 0.56, 0, 1, 0, 0.56, 0],
[0.8, 0, 0, 0, 0.8, 0, 0, 0, 0.8, 0, 0, 0, 0.8, 0, 0, 0],
[0.2, 0.3, 0.4, 0.5, 0.2, 0.3, 0.4, 0.5, 0.2, 0.3, 0.4, 0.5, 0.2, 0.3, 0.4, 0.5]]],
Agent(Point(0, 0, 0), radius=1), Goal(Point(0, 0, 1)), mutable=False)
cls.ogm_3d = OccupancyGridMap([[ # used for dynamic growth debugging
[0, 0, 0.2, 0.3, 0.4, 0.8, 1, 1, 0.8, 0, 0, 1, 1, 0, 0, 0, 0, 0.9],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.5]]] +
[[[-1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1], [-1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1]] for _ in range(40)],
Agent(Point(0, 0, 0)), Goal(Point(0, 0, 16)), unmapped_value=-1, name="Occupancy Grid 3D")
cls.grid_map_28x28vin = cls.grid_map_one_obstacle.convert_to_dense_map()
cls.grid_map_28x28vin.name = "vin test 28x28 -1"
def __update_grid(self,
weight_grid: np.ndarray,
weight_bounds: Optional[Tuple[Real, Real]] = None,
traversable_threshold: Optional[Real] = None,
unmapped_value: Optional[Real] = None) -> None:
assert weight_grid.shape == self.weight_grid.shape, "Dimension mismatch in grid update."
assert self.mutable
def remove_obstacle(p: Point):
for i in range(len(self.obstacles)):
if self.obstacles[i].position == p:
self.obstacles.pop(i)
break
# bounds
if weight_bounds is None:
ignored = [] if unmapped_value is None else [unmapped_value]
weight_bounds = (min(flatten(weight_grid, ignored)),
max(flatten(weight_grid, ignored)))
# threshold
if traversable_threshold is None:
traversable_threshold = min(
weight_bounds[0] + (weight_bounds[1] - weight_bounds[0]) *
self.DEFAULT_TRAVERSABLE_THRESHOLD, weight_bounds[1])
self.traversable_threshold = OccupancyGridMap.__normalise(
weight_bounds, traversable_threshold)
updated_cells: List[Point] = []
# todo: when obstacle modified, extended wall update required
# unmapped value
if unmapped_value is not None:
for idx in np.ndindex(*self.size):
if weight_grid[idx] == unmapped_value and self.grid[
idx] != Map.UNMAPPED_ID:
p = Point(*idx)
if self.grid[idx] == Map.WALL_ID or self.grid[
idx] == Map.EXTENDED_WALL_ID:
remove_obstacle(p)
self.grid[idx] = Map.UNMAPPED_ID
updated_cells.append(p)
# mapped values
for idx in np.ndindex(*self.size):
v = weight_grid[idx]
self.weight_grid[idx] = OccupancyGridMap.__normalise(
weight_bounds, v)
if self.grid[idx] == Map.UNMAPPED_ID and weight_grid[
idx] != unmapped_value:
p = Point(*idx)
updated_cells.append(p)
if v > traversable_threshold:
self.grid[idx] = self.WALL_ID
self.obstacles.append(Obstacle(p))
else:
self.grid[idx] = self.CLEAR_ID
elif self.grid[idx] != Map.UNMAPPED_ID:
if v > traversable_threshold:
if self.grid[idx] != Map.WALL_ID:
p = Point(*idx)
self.grid[idx] = self.WALL_ID
self.obstacles.append(Obstacle(p))
updated_cells.append(p)
elif self.grid[idx] == Map.WALL_ID:
p = Point(*idx)
self.grid[idx] = self.CLEAR_ID
remove_obstacle(p)
updated_cells.append(p)
# entities
self.grid[self.agent.position.values] = self.AGENT_ID
self.grid[self.goal.position.values] = self.GOAL_ID
if updated_cells and self.services is not None:
self.services.ev_manager.post(MapUpdateEvent(updated_cells))
class Maps:
"""
This class contains cached maps only
"""
pixel_map_small_obstacles: SparseMap = \
SparseMap(Size(100, 100),
Agent(Point(20, 60), 5),
[
Obstacle(Point(20, 50), 5),
Obstacle(Point(60, 40), 5),
Obstacle(Point(40, 80), 5),
Obstacle(Point(30, 10), 5),
Obstacle(Point(80, 80), 5),
Obstacle(Point(20, 40), 5),
],
Goal(Point(95, 95), 5))
pixel_map_empty: SparseMap = \
SparseMap(Size(500, 500),
Agent(Point(245, 245), 10),
[],
Goal(Point(0, 0), 5))
pixel_map_one_obstacle: SparseMap = \
SparseMap(Size(500, 500),
Agent(Point(40, 40), 10),
[
Obstacle(Point(250, 250), 50),
],
Goal(Point(460, 460), 10))
grid_map_one_obstacle: SparseMap = \
SparseMap(Size(32, 32),
Agent(Point(0, 15)),
[
Obstacle(Point(15, 15), 5),
],
Goal(Point(31, 15)))
grid_map_labyrinth: DenseMap = DenseMap([
[2, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 1, 3, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 1, 0, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
])
grid_map_labyrinth2: DenseMap = DenseMap([
[2, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 3, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
])
grid_map_complex_obstacle: DenseMap = DenseMap([
[2, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 1, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1, 1, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 1, 0, 1, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 1, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1, 1, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 3],
])
grid_map_small_one_obstacle: DenseMap = DenseMap([
[2, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 1, 1, 0, 0, 0],
[0, 0, 0, 1, 1, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 3],
])
grid_map_small_one_obstacle2: DenseMap = DenseMap([
[2, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 1, 1, 1, 1, 1],
[0, 0, 0, 1, 1, 1, 1, 1],
[0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 3],
])
grid_map_small_one_obstacle3: DenseMap = DenseMap([
[2, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 1, 1, 1, 1, 1],
[0, 0, 0, 1, 1, 1, 1, 1],
[0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0],
[3, 0, 0, 0, 0, 0, 0, 0],
])
grid_map_no_solution: DenseMap = DenseMap([
[0, 0, 0, 0, 0, 0, 0, 0],
[0, 1, 1, 1, 1, 1, 1, 0],
[0, 1, 0, 1, 2, 0, 1, 0],
[0, 1, 0, 1, 1, 0, 1, 0],
[0, 1, 0, 1, 1, 0, 1, 0],
[0, 1, 0, 0, 0, 0, 1, 0],
[0, 1, 1, 1, 1, 1, 1, 0],
[0, 0, 0, 0, 0, 0, 0, 3],
])
grid_yt: DenseMap = DenseMap([
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 3],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0],
[0, 0, 0, 0, 0, 0, 0, 2, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
])
def test_deep_copy(self) -> None:
entity1: Obstacle = Obstacle(Point(2, 3), 10)
entity2: Obstacle = copy.deepcopy(entity1)
self.assertEqual(entity1, entity2)
def test_str(self) -> None:
entity: Obstacle = Obstacle(Point(2, 3), 10)
self.assertEqual("Obstacle: {position: Point(2, 3), radius: 10}",
str(entity))
def test_eq(self) -> None:
entity1: Obstacle = Obstacle(Point(2, 3), 10)
entity2: Obstacle = Obstacle(Point(2, 3), 10)
self.assertEqual(entity1, entity2)
def test_ne_instance(self) -> None:
entity1: Obstacle = Obstacle(Point(2, 3), 10)
entity2: Entity = Entity(Point(2, 3), 10)
self.assertNotEqual(entity1, entity2)
def test_ne_all(self) -> None:
entity1: Obstacle = Obstacle(Point(2, 3), 10)
entity2: Obstacle = Obstacle(Point(2, 15), 15)
self.assertNotEqual(entity1, entity2)