def test_is_goal_reached_out_of_bounds(self) -> None:
map1 = DenseMap([
[DenseMap.WALL_ID, DenseMap.WALL_ID, DenseMap.WALL_ID, DenseMap.WALL_ID],
[DenseMap.AGENT_ID, DenseMap.CLEAR_ID, DenseMap.CLEAR_ID, DenseMap.CLEAR_ID],
[DenseMap.CLEAR_ID, DenseMap.CLEAR_ID, DenseMap.CLEAR_ID, DenseMap.GOAL_ID],
])
self.assertFalse(map1.is_goal_reached(Point(-1, -1)))
def test_ne_size(self) -> None:
map1: DenseMap = DenseMap([
[DenseMap.WALL_ID, DenseMap.WALL_ID, DenseMap.WALL_ID, DenseMap.WALL_ID],
[DenseMap.AGENT_ID, DenseMap.CLEAR_ID, DenseMap.WALL_ID, DenseMap.EXTENDED_WALL_ID],
[DenseMap.CLEAR_ID, DenseMap.CLEAR_ID, DenseMap.CLEAR_ID, DenseMap.GOAL_ID],
])
map2: DenseMap = DenseMap([
[DenseMap.CLEAR_ID, DenseMap.WALL_ID, DenseMap.WALL_ID, DenseMap.WALL_ID],
[DenseMap.AGENT_ID, DenseMap.CLEAR_ID, DenseMap.WALL_ID, DenseMap.EXTENDED_WALL_ID],
])
self.assertNotEqual(map1, map2)
def convert_to_dense_map(self) -> DenseMap:
"""
Converts current map into a :class:`DenseMap`
:return: The converted map
"""
grid: List[List[int]] = [[0 for _ in range(self.size.width)]
for _ in range(self.size.height)]
should_optimize: bool = len(self.obstacles) > 20
for obstacle in self.obstacles:
total_radius: int = self.agent.radius + obstacle.radius
if should_optimize:
total_radius = obstacle.radius
for x in range(obstacle.position.x - total_radius,
obstacle.position.x + total_radius + 1):
for y in range(obstacle.position.y - total_radius,
obstacle.position.y + total_radius + 1):
if not self.is_out_of_bounds_pos(Point(x, y)):
dist: Union[float, np.ndarray] = np.linalg.norm(
np.array([x, y]) - np.array(obstacle.position))
if not should_optimize and \
dist <= obstacle.radius + self.agent.radius and grid[y][x] == DenseMap.CLEAR_ID:
grid[y][x] = DenseMap.EXTENDED_WALL
if dist <= obstacle.radius:
grid[y][x] = DenseMap.WALL_ID
grid[self.agent.position.y][self.agent.position.x] = DenseMap.AGENT_ID
grid[self.goal.position.y][self.goal.position.x] = DenseMap.GOAL_ID
dense_map: DenseMap = DenseMap(grid, self._services)
dense_map.agent = copy.deepcopy(self.agent)
dense_map.goal = copy.deepcopy(self.goal)
dense_map.trace = copy.deepcopy(self.trace)
if should_optimize:
dense_map.extend_walls()
return dense_map
def test_neighbours_all(self) -> None:
map1: DenseMap = DenseMap([
[
DenseMap.WALL_ID, DenseMap.CLEAR_ID, DenseMap.CLEAR_ID,
DenseMap.CLEAR_ID
],
[
DenseMap.CLEAR_ID, DenseMap.CLEAR_ID, DenseMap.AGENT_ID,
DenseMap.CLEAR_ID
],
[
DenseMap.CLEAR_ID, DenseMap.CLEAR_ID, DenseMap.CLEAR_ID,
DenseMap.GOAL_ID
],
])
ns: List[Point] = map1.get_next_positions(Point(1, 1))
self.assertEqual(
{
Point(x=1, y=0),
Point(x=2, y=0),
Point(x=2, y=1),
Point(x=2, y=2),
Point(x=1, y=2),
Point(x=0, y=2),
Point(x=0, y=1)
}, set(ns))
def test_str_debug_level_3(self) -> None:
service: Services = Mock()
service.settings.simulator_write_debug_level = DebugLevel.HIGH
map1: SparseMap = DenseMap(
[[[DenseMap.CLEAR_ID, DenseMap.CLEAR_ID, DenseMap.CLEAR_ID],
[DenseMap.AGENT_ID, DenseMap.CLEAR_ID, DenseMap.CLEAR_ID]],
[[DenseMap.CLEAR_ID, DenseMap.WALL_ID, DenseMap.WALL_ID],
[DenseMap.CLEAR_ID, DenseMap.CLEAR_ID,
DenseMap.GOAL_ID]]]).convert_to_sparse_map()
map1._services = service
self.assertEqual(
"""SparseMap: {
size: Size(3, 2, 2),
agent: Agent: {position: Point(0, 1, 0), radius: 0},
obstacles: {
size: 2,
entities: [
Obstacle: {position: Point(1, 0, 1), radius: 0},
Obstacle: {position: Point(2, 0, 1), radius: 0},
]
},
goal: Goal: {position: Point(2, 1, 1), radius: 0}
}""", str(map1))
def test_is_goal_reached_normal(self) -> None:
map1: DenseMap = DenseMap([
[DenseMap.WALL_ID, DenseMap.WALL_ID, DenseMap.WALL_ID, DenseMap.WALL_ID],
[DenseMap.AGENT_ID, DenseMap.CLEAR_ID, DenseMap.CLEAR_ID, DenseMap.CLEAR_ID],
[DenseMap.CLEAR_ID, DenseMap.CLEAR_ID, DenseMap.CLEAR_ID, DenseMap.GOAL_ID],
])
self.assertTrue(map1.is_goal_reached(Point(3, 2)))
def test_is_goal_reached_false(self) -> None:
map1: SparseMap = DenseMap([
[DenseMap.WALL_ID, DenseMap.WALL_ID, DenseMap.WALL_ID, DenseMap.WALL_ID],
[DenseMap.AGENT_ID, DenseMap.CLEAR_ID, DenseMap.CLEAR_ID, DenseMap.CLEAR_ID],
[DenseMap.CLEAR_ID, DenseMap.CLEAR_ID, DenseMap.CLEAR_ID, DenseMap.GOAL_ID],
]).convert_to_sparse_map()
self.assertFalse(map1.is_goal_reached(Point(2, 2)))
def test_ne_sparse(self) -> None:
map1: DenseMap = DenseMap([
[[DenseMap.WALL_ID, DenseMap.WALL_ID, DenseMap.WALL_ID],
[DenseMap.AGENT_ID, DenseMap.CLEAR_ID,
DenseMap.EXTENDED_WALL_ID]],
[[DenseMap.WALL_ID, DenseMap.WALL_ID, DenseMap.WALL_ID],
[DenseMap.GOAL_ID, DenseMap.CLEAR_ID, DenseMap.CLEAR_ID]]
])
map2: DenseMap = DenseMap(
[[[DenseMap.WALL_ID, DenseMap.WALL_ID, DenseMap.WALL_ID],
[DenseMap.AGENT_ID, DenseMap.WALL_ID,
DenseMap.EXTENDED_WALL_ID]],
[[DenseMap.WALL_ID, DenseMap.WALL_ID, DenseMap.WALL_ID],
[DenseMap.GOAL_ID, DenseMap.WALL_ID,
DenseMap.WALL_ID]]]).convert_to_sparse_map()
self.assertNotEqual(map1, map2)
def convert_to_dense_map(self) -> Optional[DenseMap]:
"""
Converts current map into a :class:`DenseMap`
:return: The converted map
"""
grid: np.array = np.zeros(self.size, dtype=np.int32)
should_optimize: bool = len(self.obstacles) > 20
for obstacle in self.obstacles:
total_radius: int = self.agent.radius + obstacle.radius
if should_optimize:
total_radius = obstacle.radius
for index in np.ndindex(*(len(self.size) * [total_radius*2 + 1])):
p = [elem + obstacle.position[i] - total_radius for i, elem in enumerate(index)]
if not self.is_out_of_bounds_pos(Point(*p)):
dist: Union[float, np.ndarray] = np.linalg.norm(np.array(p) - np.array(obstacle.position))
if (not should_optimize) and (dist <= obstacle.radius + self.agent.radius) and (grid[tuple(p)] == DenseMap.CLEAR_ID):
grid[tuple(p)] = DenseMap.EXTENDED_WALL_ID
if dist <= obstacle.radius:
grid[tuple(p)] = DenseMap.WALL_ID
grid[self.agent.position.values] = DenseMap.AGENT_ID
grid[self.goal.position.values] = DenseMap.GOAL_ID
dense_map: DenseMap = DenseMap(grid, self.services, transpose=False)
dense_map.agent = copy.deepcopy(self.agent)
dense_map.goal = copy.deepcopy(self.goal)
dense_map.trace = copy.deepcopy(self.trace)
if should_optimize:
dense_map.extend_walls()
return dense_map
def test_is_valid_position_invalid(self) -> None:
map1: SparseMap = DenseMap([
[DenseMap.EXTENDED_WALL_ID, DenseMap.WALL_ID, DenseMap.WALL_ID, DenseMap.WALL_ID],
[DenseMap.AGENT_ID, DenseMap.CLEAR_ID, DenseMap.CLEAR_ID, DenseMap.WALL_ID],
[DenseMap.CLEAR_ID, DenseMap.CLEAR_ID, DenseMap.CLEAR_ID, DenseMap.GOAL_ID],
]).convert_to_sparse_map()
self.assertFalse(map1.is_agent_valid_pos(Point(1, 0)))
self.assertFalse(map1.is_agent_valid_pos(Point(-1, -1)))
def test_is_goal_reached_out_of_bounds(self) -> None:
map1: SparseMap = DenseMap(
[[[DenseMap.CLEAR_ID, DenseMap.CLEAR_ID, DenseMap.CLEAR_ID],
[DenseMap.AGENT_ID, DenseMap.CLEAR_ID, DenseMap.CLEAR_ID]],
[[DenseMap.CLEAR_ID, DenseMap.WALL_ID, DenseMap.WALL_ID],
[DenseMap.CLEAR_ID, DenseMap.CLEAR_ID,
DenseMap.GOAL_ID]]]).convert_to_sparse_map()
self.assertFalse(map1.is_goal_reached(Point(-1, -1, -1)))
def test_is_goal_reached_normal(self) -> None:
map1: SparseMap = DenseMap(
[[[DenseMap.CLEAR_ID, DenseMap.CLEAR_ID, DenseMap.CLEAR_ID],
[DenseMap.AGENT_ID, DenseMap.CLEAR_ID, DenseMap.CLEAR_ID]],
[[DenseMap.CLEAR_ID, DenseMap.WALL_ID, DenseMap.WALL_ID],
[DenseMap.CLEAR_ID, DenseMap.CLEAR_ID,
DenseMap.GOAL_ID]]]).convert_to_sparse_map()
self.assertTrue(map1.is_goal_reached(Point(2, 1, 1)))
def test_move_agent_no_trace(self) -> None:
map1: SparseMap = DenseMap([
[DenseMap.WALL_ID, DenseMap.WALL_ID, DenseMap.WALL_ID, DenseMap.WALL_ID],
[DenseMap.AGENT_ID, DenseMap.CLEAR_ID, DenseMap.CLEAR_ID, DenseMap.CLEAR_ID],
[DenseMap.CLEAR_ID, DenseMap.CLEAR_ID, DenseMap.CLEAR_ID, DenseMap.GOAL_ID],
]).convert_to_sparse_map()
map1.move_agent(Point(1, 1), True)
self.assertEqual(Point(1, 1), map1.agent.position)
self.assertEqual([], map1.trace)
def test_move_agent_out_of_bounds(self) -> None:
map1: DenseMap = DenseMap([
[DenseMap.WALL_ID, DenseMap.WALL_ID, DenseMap.WALL_ID, DenseMap.WALL_ID],
[DenseMap.AGENT_ID, DenseMap.CLEAR_ID, DenseMap.CLEAR_ID, DenseMap.CLEAR_ID],
[DenseMap.CLEAR_ID, DenseMap.CLEAR_ID, DenseMap.CLEAR_ID, DenseMap.GOAL_ID],
])
map1.move_agent(Point(-1, 0))
self.assertEqual(DenseMap.AGENT_ID, map1.at(Point(0, 1)))
self.assertEqual([Trace(Point(0, 1))], map1.trace)
def test_is_goal_reached_false(self) -> None:
map1: DenseMap = DenseMap([
[[DenseMap.CLEAR_ID, DenseMap.WALL_ID, DenseMap.WALL_ID],
[DenseMap.AGENT_ID, DenseMap.CLEAR_ID,
DenseMap.EXTENDED_WALL_ID]],
[[DenseMap.WALL_ID, DenseMap.WALL_ID, DenseMap.WALL_ID],
[DenseMap.GOAL_ID, DenseMap.CLEAR_ID, DenseMap.CLEAR_ID]]
])
self.assertFalse(map1.is_goal_reached(Point(0, 0, 0)))
def test_is_valid_position_normal(self) -> None:
map1: DenseMap = DenseMap([
[DenseMap.WALL_ID, DenseMap.WALL_ID, DenseMap.WALL_ID, DenseMap.WALL_ID],
[DenseMap.AGENT_ID, DenseMap.CLEAR_ID, DenseMap.CLEAR_ID, DenseMap.CLEAR_ID],
[DenseMap.CLEAR_ID, DenseMap.CLEAR_ID, DenseMap.CLEAR_ID, DenseMap.GOAL_ID],
])
self.assertTrue(map1.is_agent_valid_pos(Point(1, 1)))
self.assertTrue(map1.is_agent_valid_pos(Point(0, 1)))
self.assertTrue(map1.is_agent_valid_pos(Point(3, 2)))
def test_move_agent_out_of_bounds(self) -> None:
map1: SparseMap = DenseMap([
[DenseMap.WALL_ID, DenseMap.WALL_ID, DenseMap.WALL_ID, DenseMap.WALL_ID],
[DenseMap.AGENT_ID, DenseMap.CLEAR_ID, DenseMap.CLEAR_ID, DenseMap.CLEAR_ID],
[DenseMap.CLEAR_ID, DenseMap.CLEAR_ID, DenseMap.CLEAR_ID, DenseMap.GOAL_ID],
]).convert_to_sparse_map()
map1.move_agent(Point(-1, 0))
self.assertEqual(Point(0, 1), map1.agent.position)
self.assertEqual([Trace(Point(0, 1))], map1.trace)
def test_is_goal_reached_normal(self) -> None:
map1: DenseMap = DenseMap([
[[DenseMap.CLEAR_ID, DenseMap.WALL_ID, DenseMap.WALL_ID],
[DenseMap.AGENT_ID, DenseMap.CLEAR_ID,
DenseMap.EXTENDED_WALL_ID]],
[[DenseMap.WALL_ID, DenseMap.WALL_ID, DenseMap.WALL_ID],
[DenseMap.GOAL_ID, DenseMap.CLEAR_ID, DenseMap.CLEAR_ID]]
])
# TODO: this position should be (1, 1, 0) - transposed?
self.assertTrue(map1.is_goal_reached(Point(0, 1, 1)))
def test_move_agent_normal(self) -> None:
map1: DenseMap = DenseMap(
[[[DenseMap.CLEAR_ID, DenseMap.CLEAR_ID, DenseMap.CLEAR_ID],
[DenseMap.AGENT_ID, DenseMap.CLEAR_ID, DenseMap.CLEAR_ID]],
[[DenseMap.CLEAR_ID, DenseMap.WALL_ID, DenseMap.WALL_ID],
[DenseMap.CLEAR_ID, DenseMap.CLEAR_ID,
DenseMap.GOAL_ID]]]).convert_to_sparse_map()
map1.move_agent(Point(0, 0, 0))
self.assertEqual(Point(0, 0, 0), map1.agent.position)
self.assertTrue([Trace(Point(0, 0, 0))], map1.trace)
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_move_agent_normal(self) -> None:
map1: DenseMap = DenseMap([
[DenseMap.WALL_ID, DenseMap.WALL_ID, DenseMap.WALL_ID, DenseMap.WALL_ID],
[DenseMap.AGENT_ID, DenseMap.CLEAR_ID, DenseMap.CLEAR_ID, DenseMap.CLEAR_ID],
[DenseMap.CLEAR_ID, DenseMap.CLEAR_ID, DenseMap.CLEAR_ID, DenseMap.GOAL_ID],
])
map1.move_agent(Point(1, 1))
self.assertEqual(Point(1, 1), map1.agent.position)
self.assertEqual(DenseMap.AGENT_ID, map1.at(Point(1, 1)))
self.assertTrue([Trace(Point(1, 1))], map1.trace)
def test_ne_instance(self) -> None:
map1: DenseMap = DenseMap([
[[DenseMap.WALL_ID, DenseMap.WALL_ID, DenseMap.WALL_ID],
[DenseMap.AGENT_ID, DenseMap.CLEAR_ID,
DenseMap.EXTENDED_WALL_ID]],
[[DenseMap.WALL_ID, DenseMap.WALL_ID, DenseMap.WALL_ID],
[DenseMap.GOAL_ID, DenseMap.CLEAR_ID, DenseMap.CLEAR_ID]]
])
map2: int = 3
self.assertNotEqual(map1, map2)
def test_is_valid_position_normal(self) -> None:
map1: SparseMap = DenseMap([
[DenseMap.EXTENDED_WALL_ID, DenseMap.WALL_ID, DenseMap.WALL_ID, DenseMap.WALL_ID],
[DenseMap.AGENT_ID, DenseMap.CLEAR_ID, DenseMap.CLEAR_ID, DenseMap.CLEAR_ID],
[DenseMap.CLEAR_ID, DenseMap.CLEAR_ID, DenseMap.CLEAR_ID, DenseMap.GOAL_ID],
]).convert_to_sparse_map()
self.assertTrue(map1.is_agent_valid_pos(Point(1, 1)))
self.assertTrue(map1.is_agent_valid_pos(Point(0, 1)))
self.assertTrue(map1.is_agent_valid_pos(Point(3, 2)))
self.assertTrue(map1.is_agent_valid_pos(Point(0, 0)))
def _unjson(file_name: str, directory: str) -> Any:
file_name = Directory._add_extension(file_name, "json")
if not os.path.isfile(directory + file_name):
return None
dic = json.load(open(directory + file_name))
if dic['type'] == "DenseMap" and dic['version'] <= 1:
return DenseMap(dic['grid'])
return None
def test_move_agent_no_trace(self) -> None:
map1: DenseMap = DenseMap([
[[DenseMap.CLEAR_ID, DenseMap.WALL_ID, DenseMap.WALL_ID],
[DenseMap.AGENT_ID, DenseMap.CLEAR_ID,
DenseMap.EXTENDED_WALL_ID]],
[[DenseMap.WALL_ID, DenseMap.WALL_ID, DenseMap.WALL_ID],
[DenseMap.GOAL_ID, DenseMap.CLEAR_ID, DenseMap.CLEAR_ID]]
])
map1.move_agent(Point(0, 1, 1), True)
self.assertEqual(Point(0, 1, 1), map1.agent.position)
self.assertEqual(DenseMap.AGENT_ID, map1.grid[0, 1, 1])
self.assertEqual([], map1.trace)
def test_move_agent_out_of_bounds(self) -> None:
map1: DenseMap = DenseMap([
[[DenseMap.CLEAR_ID, DenseMap.WALL_ID, DenseMap.WALL_ID],
[DenseMap.AGENT_ID, DenseMap.CLEAR_ID,
DenseMap.EXTENDED_WALL_ID]],
[[DenseMap.WALL_ID, DenseMap.WALL_ID, DenseMap.WALL_ID],
[DenseMap.GOAL_ID, DenseMap.CLEAR_ID, DenseMap.CLEAR_ID]]
])
map1.move_agent(Point(-1, 0, 0))
self.assertEqual(Point(0, 1, 0), map1.agent.position)
self.assertEqual(DenseMap.AGENT_ID, map1.grid[0, 1, 0])
self.assertEqual([Trace(Point(0, 1, 0))], map1.trace)
def __generate_random_map(self, dimensions: Size, obstacle_fill_rate: float = 0.3) -> Map:
grid: List[List[int]] = [[0 for _ in range(dimensions.width)] for _ in range(dimensions.height)]
fill: float = dimensions.width * dimensions.height * obstacle_fill_rate
nr_of_obstacles = 0
while nr_of_obstacles < fill:
obst_pos: Point = self.__get_rand_position(dimensions)
if grid[obst_pos.y][obst_pos.x] == DenseMap.CLEAR_ID:
grid[obst_pos.y][obst_pos.x] = DenseMap.WALL_ID
nr_of_obstacles += 1
self.__place_random_agent_and_goal(grid, dimensions)
return DenseMap(grid)
def test_str(self) -> None:
map1: DenseMap = DenseMap([
[DenseMap.EXTENDED_WALL_ID, DenseMap.WALL_ID, DenseMap.WALL_ID, DenseMap.WALL_ID],
[DenseMap.AGENT_ID, DenseMap.CLEAR_ID, DenseMap.CLEAR_ID, DenseMap.WALL_ID],
[DenseMap.CLEAR_ID, DenseMap.CLEAR_ID, DenseMap.CLEAR_ID, DenseMap.GOAL_ID],
])
self.assertEqual("""DenseMap: {
size: Size(4, 3),
agent: Agent: {position: Point(0, 1), radius: 0},
goal: Goal: {position: Point(3, 2), radius: 0},
obstacles: 5,
grid: [
4, 1, 1, 1,
2, 0, 0, 1,
0, 0, 0, 3,
]
}""", str(map1))
def __generate_random_map(self,
dimensions: Size,
obstacle_fill_rate: float = 0.3) -> Map:
grid: np.array = np.zeros(dimensions)
fill: float = math.prod(dimensions) * obstacle_fill_rate
nr_of_obstacles = 0
while nr_of_obstacles < fill:
obst_pos: Point = self.__get_rand_position(dimensions)
if grid[obst_pos.values] == DenseMap.CLEAR_ID:
grid[obst_pos.values] = DenseMap.WALL_ID
nr_of_obstacles += 1
self.__place_random_agent_and_goal(grid, dimensions)
return DenseMap(grid)
def test_neighbours_bounds(self) -> None:
map1: DenseMap = DenseMap([
[
DenseMap.WALL_ID, DenseMap.WALL_ID, DenseMap.WALL_ID,
DenseMap.WALL_ID
],
[
DenseMap.AGENT_ID, DenseMap.CLEAR_ID, DenseMap.WALL_ID,
DenseMap.EXTENDED_WALL_ID
],
[
DenseMap.CLEAR_ID, DenseMap.CLEAR_ID, DenseMap.CLEAR_ID,
DenseMap.GOAL_ID
],
])
ns = map1.get_next_positions(Point(0, 1))
self.assertEqual({Point(1, 1), Point(1, 2), Point(0, 2)}, set(ns))
