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_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_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 _setup_sim(self,
config: Optional[Configuration] = None,
goal: Optional[Point] = None) -> Simulator:
"""
Sets up the simulator (e.g. algorithm and map configuration).
"""
while self.grid is None or self.agent is None:
rospy.loginfo("Waiting for grid and agent to initialise...")
rospy.sleep(0.5)
if config is None:
config = Configuration()
# general
config.simulator_graphics = True
config.simulator_key_frame_speed = 0.16
config.simulator_key_frame_skip = 20
config.get_agent_position = lambda: self._world_to_grid(
Point(self.agent.pose.position.x, self.agent.pose.position.y))
config.visualiser_simulator_config = False # hide the simulator config window
# algorithm
if config.algorithm_name is None:
config.algorithm_name = "WPN-view"
config.simulator_algorithm_type, config.simulator_testing_type, config.simulator_algorithm_parameters = config.algorithms[
config.algorithm_name]
# map
goal = Goal(Point(0, 0) if goal is None else goal)
agent = Agent(self._world_to_grid(
Point(self.agent.pose.position.x, self.agent.pose.position.y)),
radius=self.INFLATE)
mp = RosMap(agent,
goal,
lambda: self.grid,
traversable_threshold=self.TRAVERSABLE_THRESHOLD,
unmapped_value=-1,
wp_publish=self._send_way_point,
update_requested=self._map_update_requested,
name="ROS Map")
config.maps = {mp.name: mp}
config.simulator_initial_map = list(config.maps.values())[0]
config.map_name = list(config.maps.keys())[0]
# create the simulator
s = Services(config)
s.algorithm.map.request_update()
sim = Simulator(s)
return sim
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 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 _setup_sim(self, config: Optional[Configuration] = None) -> Simulator:
"""
Sets up the simulator (e.g. algorithm and map configuration).
"""
while self.grid is None:
rospy.loginfo("Waiting for grid to initialise...")
rospy.sleep(0.5)
if config is None:
config = Configuration()
# general
config.simulator_graphics = True
config.simulator_key_frame_speed = 0.16
config.simulator_key_frame_skip = 20
config.visualiser_simulator_config = False # hide the simulator config window
# algorithm
if config.algorithm_name is None:
config.algorithm_name = "A*"
config.simulator_algorithm_type, config.simulator_testing_type, config.simulator_algorithm_parameters = config.algorithms[
config.algorithm_name]
# map
mp = RosMap(Agent(Point(40, 40)),
Goal(Point(30, 20)),
lambda: self.grid,
traversable_threshold=0.4,
name="ROS Map")
config.maps = {mp.name: mp}
config.simulator_initial_map = list(config.maps.values())[0]
config.map_name = list(config.maps.keys())[0]
# create the simulator
s = Services(config)
s.algorithm.map.request_update()
sim = Simulator(s)
return sim
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_instance(self) -> None:
entity1: Agent = Agent(Point(2, 3), 10)
entity2: Entity = Entity(Point(2, 3), 10)
self.assertNotEqual(entity1, entity2)
def test_ne_all(self) -> None:
entity1: Agent = Agent(Point(2, 3), 10)
entity2: Agent = Agent(Point(2, 15), 15)
self.assertNotEqual(entity1, entity2)
def test_eq(self) -> None:
entity1: Agent = Agent(Point(2, 3), 10)
entity2: Agent = Agent(Point(2, 3), 10)
self.assertEqual(entity1, entity2)
def test_str(self) -> None:
entity: Agent = Agent(Point(2, 3), 10)
self.assertEqual("Agent: {position: Point(2, 3), radius: 10}",
str(entity))
def test_deep_copy(self) -> None:
entity1: Agent = Agent(Point(2, 3), 10)
entity2: Agent = copy.deepcopy(entity1)
self.assertEqual(entity1, entity2)
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"