def test_plan(debug=False):
occupancy_map_data = load_occupancy_map_data('test', 'frontier_plan_map.png')
occupancy_map = Costmap(data=occupancy_map_data, resolution=0.03, origin=np.array([-6.305, -6.305]))
pose = np.array([.8, 0, -0.51759265])
frontier_agent = create_frontier_agent_from_params(os.path.join(get_exploration_dir(), "params/params.yaml"))
frontier_agent.is_first_plan = False
plan = frontier_agent.plan(pose, occupancy_map)
if debug:
visualize(occupancy_map, pose, np.array([]), np.array([]), frontier_agent.get_footprint(), [], (1000, 1000), pose,
frontiers=frontier_agent.get_frontiers(compute=True, occupancy_map=occupancy_map))
assert plan.shape[0] > 2
def test_footprint_orientation(debug=False):
state = np.array([0.71, 0.35, np.pi / 4])
map_data = load_occupancy_map_data('test', 'footprint_orientation_map.png')
occupancy_map = Costmap(data=map_data,
resolution=0.03,
origin=[-19.72, -1.54])
footprint_points = get_tricky_circular_footprint()
footprint = CustomFootprint(footprint_points,
2 * np.pi / 180.,
inflation_scale=1.3)
assert not footprint.check_for_collision(state, occupancy_map, debug=debug)
assert not footprint.check_for_collision(
state, occupancy_map, debug=debug, use_python=True)
def test_oriented_astar(debug=False):
occupancy_map_data = load_occupancy_map_data('brain', 'big_retail.png')
occupancy_map = Costmap(occupancy_map_data, 0.03, origin=[0., 0.])
footprint_points = get_tricky_circular_footprint()
footprint = CustomFootprint(footprint_points,
2. * np.pi / 180.,
inflation_scale=2.0)
angles = get_astar_angles()
footprint_masks = footprint.get_footprint_masks(occupancy_map.resolution,
angles=angles)
outline_coords = footprint.get_outline_coords(occupancy_map.resolution,
angles=angles)
start = rc_to_xy([1956, 137, 0], occupancy_map)
goal = rc_to_xy([841, 3403, 0.], occupancy_map)
start_time = time.time()
success, path = oriented_astar(goal=goal,
start=start,
occupancy_map=occupancy_map,
footprint_masks=footprint_masks,
outline_coords=outline_coords,
obstacle_values=[0, 127],
planning_scale=10)
time_elapsed = time.time() - start_time
assert success
if debug:
visualization_map = occupancy_map.copy()
visualization_map.data = np.dstack(
(occupancy_map.data, occupancy_map.data, occupancy_map.data))
draw_footprint_path(footprint, path, visualization_map, [255, 0, 0],
[0, 0, 255])
if success:
print("found solution in", time_elapsed, "seconds, with length:",
path.shape[0])
else:
print("failed to find solution")
plt.imshow(visualization_map.data, interpolation='nearest')
plt.show()
def test_extract_frontiers(debug=False):
occupancy_map_data = load_occupancy_map_data('test', 'vw_partly_explored_test.png')
occupancy_map = Costmap(data=occupancy_map_data, resolution=0.03, origin=np.array([0, 0]))
frontiers = extract_frontiers(occupancy_map=occupancy_map, approx=False,
kernel=cv2.getStructuringElement(cv2.MORPH_CROSS, (3, 3)))
frontier_sizes = np.array([frontier.shape[0] if len(frontier.shape) > 1 else 1 for frontier in frontiers])
significant_frontiers = [frontier for i, frontier in enumerate(frontiers) if frontier_sizes[i] > 70]
assert len(significant_frontiers) == 4
if debug:
frontier_map = np.repeat([occupancy_map.data], repeats=3, axis=0).transpose((1, 2, 0))
for frontier in significant_frontiers:
frontier_px = xy_to_rc(frontier, occupancy_map).astype(np.int)
frontier_px = frontier_px[which_coords_in_bounds(frontier_px, occupancy_map.get_shape())]
frontier_map[frontier_px[:, 0], frontier_px[:, 1]] = [255, 0, 0]
plt.imshow(frontier_map, cmap='gray', interpolation='nearest')
plt.show()
def test_multithread_astar(debug=False):
num_instances = 10
pool = multiprocessing.Pool()
occupancy_map_data = load_occupancy_map_data('test', 'maze_small.png')
occupancy_map = Costmap(occupancy_map_data, 0.03, origin=[0., 0.])
obstacle_values = np.array([Costmap.OCCUPIED, Costmap.UNEXPLORED],
dtype=np.uint8)
start = rc_to_xy(
np.argwhere(occupancy_map.data == Costmap.FREE)[0, :], occupancy_map)
goals = rc_to_xy(
np.argwhere(occupancy_map.data == Costmap.FREE)[-num_instances:, :],
occupancy_map)
astar_fn = partial(astar,
start=start,
occupancy_map=occupancy_map,
obstacle_values=obstacle_values)
start_time = time.time()
results = pool.map(astar_fn, goals)
time_elapsed = time.time() - start_time
if debug:
print("ran", num_instances, "astar instances, finished in",
time_elapsed)
for i, [successful, path] in enumerate(results):
assert successful
if debug:
if successful:
print(" instance", i, "\b: found solution with length:",
path.shape[0])
else:
print(" instance", i, "\b: failed to find solution")
def test_one_astar(debug=False):
occupancy_map_data = load_occupancy_map_data('test', 'maze_small.png')
occupancy_map = Costmap(occupancy_map_data, 0.03, origin=[0., 0.])
obstacle_values = np.array([Costmap.OCCUPIED, Costmap.UNEXPLORED],
dtype=np.uint8)
start = rc_to_xy(
np.argwhere(occupancy_map.data == Costmap.FREE)[0, :], occupancy_map)
goal = rc_to_xy(
np.argwhere(occupancy_map.data == Costmap.FREE)[-1, :], occupancy_map)
start_time = time.time()
success, path = astar(goal=goal,
start=start,
occupancy_map=occupancy_map,
obstacle_values=obstacle_values,
allow_diagonal=False)
time_elapsed = time.time() - start_time
assert success
if debug:
map_solution = np.dstack(
(occupancy_map.data, occupancy_map.data, occupancy_map.data))
path_px = xy_to_rc(path, occupancy_map).astype(np.int)
map_solution[path_px[:, 0], path_px[:, 1]] = [0, 0, 255]
if success:
print("found solution in", time_elapsed, "seconds, with length:",
path.shape[0])
else:
print("failed to find solution")
plt.imshow(map_solution, interpolation='nearest')
plt.show()
def test_custom_footprint(debug=False):
map_data = load_occupancy_map_data('test', 'three_rooms.png')
occupancy_map = Costmap(data=map_data, resolution=0.03, origin=[0, 0])
free_state = rc_to_xy(np.array([110, 220, 0]), occupancy_map)
colliding_state = rc_to_xy(np.array([90, 215, 0]), occupancy_map)
footprint_points = get_tricky_oval_footprint()
footprint = CustomFootprint(footprint_points, 2 * np.pi / 180.)
assert not footprint.check_for_collision(
free_state, occupancy_map, debug=debug)
assert footprint.check_for_collision(colliding_state,
occupancy_map,
debug=debug)
assert not footprint.check_for_collision(
free_state, occupancy_map, debug=debug, use_python=True)
assert footprint.check_for_collision(colliding_state,
occupancy_map,
debug=debug,
use_python=True)
rotated_colliding_state = rc_to_xy(np.array([110, 220, -np.pi / 2]),
occupancy_map)
assert footprint.check_for_collision(rotated_colliding_state,
occupancy_map,
debug=debug)
assert footprint.check_for_collision(rotated_colliding_state,
occupancy_map,
debug=debug,
use_python=True)
footprint_points = get_tricky_circular_footprint()
footprint = CustomFootprint(footprint_points, 2 * np.pi / 180.)
assert not footprint.check_for_collision(
free_state, occupancy_map, debug=debug)
assert footprint.check_for_collision(colliding_state,
occupancy_map,
debug=debug)
assert not footprint.check_for_collision(
free_state, occupancy_map, debug=debug, use_python=True)
assert footprint.check_for_collision(colliding_state,
occupancy_map,
debug=debug,
use_python=True)
rotated_colliding_state = rc_to_xy(np.array([115, 155, np.pi / 4]),
occupancy_map)
assert footprint.check_for_collision(rotated_colliding_state,
occupancy_map,
debug=debug)
assert footprint.check_for_collision(rotated_colliding_state,
occupancy_map,
debug=debug,
use_python=True)
rotated_colliding_state = rc_to_xy(np.array([0, 0, np.pi / 2]),
occupancy_map)
assert footprint.check_for_collision(rotated_colliding_state,
occupancy_map,
debug=debug)
assert footprint.check_for_collision(rotated_colliding_state,
occupancy_map,
debug=debug,
use_python=True)