def __subdivide(top_left_corner, dim):
if dim.width < 2 * min_room_size.width - 1 and dim.height < 2 * min_room_size.height - 1:
__place_room(grid, dim, top_left_corner)
return
elif dim.width < 2 * min_room_size.width - 1:
is_vertical_split = False
elif dim.height < 2 * min_room_size.height - 1:
is_vertical_split = True
else:
is_vertical_split = int(torch.randint(0, 2, (1,)).item()) == 0
if dim.width <= max_room_size.width and dim.height <= max_room_size.height:
if int(torch.randint(0, 2, (1,)).item()) == 0:
__place_room(grid, dim, top_left_corner)
return
# split
new_top_left_corner1 = top_left_corner
if is_vertical_split:
new_dim1 = Size(__get_subdivision_number(dim, True), dim.height)
new_top_left_corner2 = Point(top_left_corner.x + new_dim1.width - 1, top_left_corner.y)
new_dim2 = Size(dim.width - new_dim1.width + 1, dim.height)
else:
new_dim1 = Size(dim.width, __get_subdivision_number(dim, False))
new_top_left_corner2 = Point(top_left_corner.x, top_left_corner.y + new_dim1.height - 1)
new_dim2 = Size(dim.width, dim.height - new_dim1.height + 1)
__subdivide(new_top_left_corner1, new_dim1)
__subdivide(new_top_left_corner2, new_dim2)
def test_ne_all(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: SparseMap = SparseMap(Size(100, 200),
Agent(Point(10, 20), 10),
[Obstacle(Point(100, 100), 35)],
Goal(Point(180, 10), 10))
self.assertNotEqual(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 _set_grid(self, msg):
minfo = msg.info
rgrid = np.array(msg.data, dtype=np.int8).astype(np.uint8)
self._resolution = minfo.resolution
self._origin = Point(minfo.origin.position.x, minfo.origin.position.y)
self._size = Size(minfo.width, minfo.height)
grid = np.empty(self._size[::-1], dtype=np.float32)
for j in range(self._size.height):
for i in range(self._size.width):
grid[j, i] = rgrid[j * self._size.width + i]
MAX_SIZE = 128
if MAX_SIZE < self._size.height or MAX_SIZE < self._size.width:
scale = MAX_SIZE / self._size.height
if (MAX_SIZE / self._size.width) < scale:
scale = MAX_SIZE / self._size.width
grid = cv.resize(grid,
None,
fx=scale,
fy=scale,
interpolation=cv.INTER_AREA)
for idx in np.ndindex(grid.shape):
grid[idx] = min(1, max(0, 1.0 - grid[idx] / 255.0))
self.grid = grid
def test_ne_instance(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: 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_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 __init__(self,
grid: Optional[List[List[int]]],
services: Services = None) -> None:
self.grid = None
super().__init__(Size(0, 0), services)
if not grid:
return
self.set_grid(grid)
def get_occupancy_percentage_grid(grid: np.array, token: int) -> float:
"""
Searches for token in grid and returns the occupancy percentage of the token
:param grid: The grid
:param token: The search token
:return: The percentage
"""
tokens: int = np.sum(grid == token)
return BasicTesting.get_occupancy_percentage_size(
Size(*grid.shape), tokens)
def __generate_random_const_obstacles(self, dimensions: Size,
obstacle_fill_rate: float,
nr_of_obstacles: int):
grid: np.array = np.zeros(dimensions)
total_fill = int(math.prod(dimensions) * obstacle_fill_rate)
print(math.prod(dimensions))
for i in range(nr_of_obstacles):
next_obst_fill = np.random.randint(total_fill)
if i == nr_of_obstacles - 1:
next_obst_fill = total_fill
if next_obst_fill == 0:
break
while True:
if (dimensions.n_dim == 3):
first_side = np.random.randint(
round(next_obst_fill**(1. / 3))) + 1
second_side = np.random.randint(
round(next_obst_fill**(1. / 3))) + 1
third_side = np.random.randint(
round(next_obst_fill**(1. / 3))) + 1
size = Size(first_side, second_side, third_side)
else:
first_side = np.random.randint(
round(next_obst_fill**(1. / 2))) + 1
second_side = max(int(next_obst_fill / first_side), 1)
size = Size(first_side, second_side)
top_left_corner = self.__get_rand_position(dimensions)
if self.__can_place_square(size, top_left_corner, dimensions):
self.__place_square(grid, size, top_left_corner)
break
total_fill -= next_obst_fill
self.__place_random_agent_and_goal(grid, dimensions)
return DenseMap(grid)
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 get_occupancy_percentage_grid(grid: List[List[int]],
token: int) -> float:
"""
Searches for token in grid and returns the occupancy percentage of the token
:param grid: The grid
:param token: The search token
:return: The percentage
"""
tokens: int = 0
width: int = len(grid)
height: int = 0 if width == 0 else len(grid[0])
for x in range(height):
for y in range(width):
tokens += grid[y][x] == token
return BasicTesting.get_occupancy_percentage_size(
Size(width, height), tokens)
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 __generate_random_const_obstacles(self, dimensions: Size, obstacle_fill_rate: float, nr_of_obstacles: int):
grid: List[List[int]] = [[0 for _ in range(dimensions.width)] for _ in range(dimensions.height)]
total_fill = int(obstacle_fill_rate * dimensions.width * dimensions.height) + 1
for i in range(nr_of_obstacles):
next_obst_fill = torch.randint(total_fill, (1,)).item()
if i == nr_of_obstacles - 1:
next_obst_fill = total_fill
if next_obst_fill == 0:
break
while True:
first_side = int(torch.randint(int(math.sqrt(next_obst_fill)) + 1, (1,)).item())
if first_side == 0:
continue
second_side = int(next_obst_fill / first_side)
size = Size(first_side, second_side)
top_left_corner = self.__get_rand_position(dimensions)
if self.__can_place_square(size, top_left_corner, dimensions):
self.__place_square(grid, size, top_left_corner)
break
total_fill -= next_obst_fill
"""
# Corner logic
agent_corner = torch.randint(4, (1,)).item()
grid = self.__place_entity_near_corner(Agent, agent_corner, grid, dimensions)
goal_corner = (4 + agent_corner - 2) % 4
grid = self.__place_entity_near_corner(Goal, goal_corner, grid, dimensions)
"""
self.__place_random_agent_and_goal(grid, dimensions)
return DenseMap(grid)
def __init__(self,
grid: Optional[List] = None,
services: Services = None,
transpose: bool = True,
mutable: bool = False,
name: Optional[str] = None) -> None:
self.grid = None
arr_grid = None
if grid is not None:
arr_grid = np.atleast_2d(np.array(grid))
if arr_grid.dtype == object:
raise ValueError(
"Cannot create DenseMap grid from ragged nested sequences (which is a list-or-tuple of lists-or-tuples-or ndarrays with different lengths or shapes)"
)
super().__init__(Size(*([0] * arr_grid.ndim)), services, mutable,
name)
else:
super().__init__(services=services, mutable=mutable, name=name)
return
# Doesn't work with non-uniform grids
self.set_grid(arr_grid, transpose)
def test_eq_3d(self) -> None:
size1: Size = Size(2, 3, 4)
size2: Size = Size(2, 3, 4)
self.assertEqual(size1, size2)
def _set_slam(self, msg: OccupancyGrid) -> None:
map_info = msg.info
grid_data = msg.data
if self._size is None: # init #
self._size = Size(self.MAP_SIZE, self.MAP_SIZE)
self._res = map_info.resolution
self._scale = self.INIT_MAP_SIZE / map_info.height
if (self.INIT_MAP_SIZE / map_info.width) < self._scale:
self._scale = self.INIT_MAP_SIZE / map_info.width
# convert raw grid data into a matrix for future processing (compression)
raw_grid = np.empty((map_info.height, map_info.width),
dtype=np.float32)
for i in range(len(grid_data)):
col = i % map_info.width
row = int((i - col) / map_info.width)
raw_grid[row, col] = grid_data[i]
# compress the map to a suitable size (maintains aspect ratio)
raw_grid = cv.resize(raw_grid,
None,
fx=self._scale,
fy=self._scale,
interpolation=cv.INTER_AREA)
if self._origin is None: # init #
# set the origin to the big map origin, use the raw grid origin with
# negative grid position to retrieve the big map's origin in world coordinates
self._origin = Point(map_info.origin.position.x,
map_info.origin.position.y)
init_map_size = Size(*raw_grid.shape[::-1])
map_origin_pos = Point(
(init_map_size.width - self._size.width) // 2,
(init_map_size.height - self._size.height) // 2)
self._origin = self._grid_to_world(map_origin_pos)
# get position of big map origin in current raw map
start = self._world_to_grid(self._origin,
origin=Point(map_info.origin.position.x,
map_info.origin.position.y))
# take an offsetted, potentially cropped view of the current raw map
grid = np.full(self._size, -1)
for i in range(start[0], start[0] + self._size.width):
for j in range(start[1], start[1] + self._size.height):
if i >= 0 and j >= 0 and i < raw_grid.shape[
1] and j < raw_grid.shape[0]:
grid[i - start[0]][j - start[1]] = raw_grid[j][i]
# hacky work-around for not having extended walls implemented. Here we manually
# extend the walls, by placing obstacles (works just as well, but should still
# ideally implement extended walls).
INVALID_VALUE = -2
grid_walls_extended = np.full(self._size, INVALID_VALUE)
for idx in np.ndindex(grid_walls_extended.shape):
if grid_walls_extended[idx] == INVALID_VALUE:
grid_walls_extended[idx] = grid[idx]
if grid[idx] > self.TRAVERSABLE_THRESHOLD:
for i in range(-self.INFLATE, self.INFLATE + 1):
for j in range(-self.INFLATE, self.INFLATE + 1):
grid_walls_extended[(idx[0] + i,
idx[1] + j)] = grid[idx]
# make new grid accessible.
# Note, it's up to the user / algorithm to request a map update for thread
# safety, e.g. via `self._sim.services.algorithm.map.request_update()`.
self.grid = grid_walls_extended
def test_ne_dim(self) -> None:
size1: Size = Size(2, 3)
size2: Size = Size(2, 3, 0)
self.assertNotEqual(size1, size2)
def test_ne_pos_3d(self) -> None:
size1: Size = Size(2, 3, 4)
size2: Size = Size(2, 3, 5)
self.assertNotEqual(size1, size2)
def test_ne(self) -> None:
self.assertNotEqual(Map(Size(3, 2)), 5)
def test_copy(self) -> None:
map1: Map = Map(Size(2, 3))
with self.assertRaises(Exception):
copy.copy(map1)
def test_ne_instance(self) -> None:
size1: int = 2
size2: Size = Size(2, 3)
self.assertNotEqual(size1, size2)
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"
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_ne_pos(self) -> None:
size1: Size = Size(2, 3)
size2: Size = Size(2, 5)
size3: Size = Size(1, 3)
self.assertNotEqual(size1, size2)
self.assertNotEqual(size1, size3)
def test_conversion(self) -> None:
size: Size = Size(2, 3, 4)
self.assertEqual(tuple(size), (2, 3, 4))
self.assertEqual(size.values, (2, 3, 4))
self.assertEqual(list(size), [2, 3, 4])
def test_copy(self) -> None:
size1: Size = Size(2, 3)
size2: Size = copy.copy(size1)
self.assertEqual(size1, size2)
def test_str_3d(self) -> None:
size = Size(2, 3, 4)
self.assertEqual("Size(2, 3, 4)", str(size))