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_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 astar(goal,
start,
occupancy_map,
obstacle_values,
planning_scale=1,
delta=0.0,
epsilon=1.0,
allow_diagonal=False):
"""
Wrapper for vanilla a-star c++ planning. given a start and end and a map, give a path.
:param goal array(3)[float]: [x, y, theta] goal pose of the robot
:param start array(3)[float]: [x, y, theta] start pose of the robot
:param occupancy_map Costmap: occupancy map used for planning, data must be compatible with uint8
:param obstacle_values array(N)[uint8]: an array containing values that the collision checker should deem as an obstacle
i.e [127, 0]
:param planning_scale int: value > 1, to plan on a lower resolution than the original occupancy map resolution,
this value is round_int(desired resolution / original resolution)
:param delta float: distance in pixels to extend the goal region for solution (allow to be within delta away from goal)
TODO FIX this to be in meters
:param epsilon float: weighting for the heuristic in the A* algorithm
:param allow_diagonal bool: whether to allow diagonal movements
:return Tuple[bool, array(N, 3)[float]]: whether we were able to successfully plan to the goal node,
and the most promising path to the goal node (solution if obtained)
"""
start_px = xy_to_rc(start, occupancy_map)
c_start = np.array(start_px[:2], dtype=np.int32)
goal_px = xy_to_rc(goal, occupancy_map)
c_goal = np.array(goal_px[:2], dtype=np.int32)
c_occupancy_map = occupancy_map.data.astype(np.uint8)
success, path_px = c_astar(c_start, c_goal, c_occupancy_map,
obstacle_values, delta, epsilon, planning_scale,
allow_diagonal)
path = rc_to_xy(path_px, occupancy_map)
return success, np.vstack(([start], path))
def extract_frontiers(occupancy_map,
approx=True,
approx_iters=2,
kernel=cv2.getStructuringElement(cv2.MORPH_CROSS,
(3, 3)),
debug=False):
"""
Given a map of free/occupied/unexplored pixels, identify the frontiers.
This method is a quicker method than the brute force approach,
and scales pretty well with large maps. Using cv2.dilate for 1 pixel on the unexplored space
and comparing with the free space, we can isolate the frontiers.
:param occupancy_map Costmap: object corresponding to the map to extract frontiers from
:param approx bool: does an approximation of the map before extracting frontiers. (dilate erode, get rid of single
pixel unexplored areas creating a large number fo frontiers)
:param approx_iters int: number of iterations for the dilate erode
:param kernel array(N, N)[float]: the kernel of which to use to extract frontiers / approx map
:param debug bool: show debug windows?
:return List[array(N, 2][float]: list of frontiers, each frontier is a set of coordinates
"""
# todo regional frontiers
# extract coordinates of occupied, unexplored, and free coordinates
occupied_coords = np.argwhere(
occupancy_map.data.astype(np.uint8) == Costmap.OCCUPIED)
unexplored_coords = np.argwhere(
occupancy_map.data.astype(np.uint8) == Costmap.UNEXPLORED)
free_coords = np.argwhere(
occupancy_map.data.astype(np.uint8) == Costmap.FREE)
if free_coords.shape[0] == 0 or unexplored_coords.shape[0] == 0:
return []
# create a binary mask of unexplored pixels, letting unexplored pixels = 1
unexplored_mask = np.zeros_like(occupancy_map.data)
unexplored_mask[unexplored_coords[:, 0], unexplored_coords[:, 1]] = 1
# dilate using a 3x3 kernel, effectively increasing
# the size of the unexplored space by one pixel in all directions
dilated_unexplored_mask = cv2.dilate(unexplored_mask, kernel=kernel)
dilated_unexplored_mask[occupied_coords[:, 0], occupied_coords[:, 1]] = 1
# create a binary mask of the free pixels
free_mask = np.zeros_like(occupancy_map.data)
free_mask[free_coords[:, 0], free_coords[:, 1]] = 1
# can isolate the frontiers using the difference between the masks,
# and looking for contours
frontier_mask = ((1 - dilated_unexplored_mask) - free_mask)
if approx:
frontier_mask = cv2.dilate(frontier_mask,
kernel=kernel,
iterations=approx_iters)
frontier_mask = cv2.erode(frontier_mask,
kernel=kernel,
iterations=approx_iters)
# this indexing will work with opencv 2.x 3.x and 4.x
frontiers_xy_px = cv2.findContours(frontier_mask, cv2.RETR_LIST,
cv2.CHAIN_APPROX_NONE)[-2:][0]
frontiers = [
rc_to_xy(np.array(frontier).squeeze(1)[:, ::-1], occupancy_map)
for frontier in frontiers_xy_px
]
if debug:
frontier_map = np.repeat([occupancy_map.data], repeats=3,
axis=0).transpose((1, 2, 0))
# frontiers = [frontiers[rank] for i, rank in enumerate(frontier_ranks)
# if i < self.num_frontiers_considered]
for frontier in frontiers:
# if frontier.astype(np.int).tolist() in self.frontier_blacklist:
# continue
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()
return frontiers
def check_for_collision(self,
state,
occupancy_map,
unexplored_is_occupied=False,
use_python=False,
debug=False):
"""
using the state and the map, check for a collision on the map.
:param state array(3)[float]: state of the robot [x, y, theta]
:param occupancy_map Costmap: object to check the footprint against
:param unexplored_is_occupied bool: whether to treat unexplored on the map as occupied
:param use_python bool: whether to use python collision checking or c++
:param debug bool: show debug plots?
:return bool: True for collision
"""
# check if we have computed a mask for this resolution, if not compute it
if occupancy_map.resolution not in list(
self._rotated_masks_set.keys()):
self._add_new_masks(occupancy_map.resolution)
# convert state to pixel coordinates
state_px = xy_to_rc(pose=state, occupancy_map=occupancy_map)
position = np.array(state_px[:2]).astype(np.int)
if debug:
map_vis = occupancy_map.copy()
map_vis.data = np.repeat([occupancy_map.data], repeats=3,
axis=0).transpose((1, 2, 0)).copy()
self.draw(rc_to_xy(state_px, occupancy_map), map_vis,
[255, 10, 10])
occupied_inds = np.argwhere(occupancy_map.data == Costmap.OCCUPIED)
map_vis.data[occupied_inds[:, 0], occupied_inds[:, 1]] = [0, 0, 0]
plt.imshow(map_vis.data, interpolation='nearest')
plt.show()
# get the mask radius that was saved for this resolution
mask_radius = self._mask_radius_set[occupancy_map.resolution]
# compute the closest angle to the current angle in the state, and get that rotated footprint
closest_angle_ind = np.argmin(np.abs(state_px[2] - self._mask_angles))
footprint_mask = self._rotated_masks_set[
occupancy_map.resolution][closest_angle_ind]
outline_coords = self._rotated_outline_coords_set[
occupancy_map.resolution][closest_angle_ind]
if not use_python:
obstacle_values = [
Costmap.OCCUPIED, Costmap.UNEXPLORED
] if unexplored_is_occupied else [Costmap.OCCUPIED]
is_colliding = check_for_collision(state,
occupancy_map,
footprint_mask=footprint_mask,
outline_coords=outline_coords,
obstacle_values=obstacle_values)
else:
# part of robot off the edge of map, it is a collision, because we assume map is bounded
if not np.all(
which_coords_in_bounds(
coords=(outline_coords + state_px[:2]).astype(np.int),
map_shape=occupancy_map.get_shape())):
return True
# get a small subsection around the state in the map (as big as our mask),
# and do a masking check with the footprint to see if there is a collision.
# if part of our subsection is off the map, we need to clip the size
# to reflect this, in both the subsection and the mask
min_range = [position[0] - mask_radius, position[1] - mask_radius]
max_range = [
position[0] + mask_radius + 1, position[1] + mask_radius + 1
]
clipped_min_range, clipped_max_range = clip_range(
min_range, max_range, occupancy_map.data.shape)
min_range_delta = clipped_min_range - np.array(min_range)
max_range_delta = clipped_max_range - np.array(max_range)
ego_map = occupancy_map.data[
clipped_min_range[0]:clipped_max_range[0],
clipped_min_range[1]:clipped_max_range[1]].copy()
# todo might be - max_range_delta
footprint_mask = footprint_mask[
min_range_delta[0]:footprint_mask.shape[1] +
max_range_delta[0],
min_range_delta[1]:footprint_mask.shape[1] +
max_range_delta[1]]
# treat unexplored space as obstacle
if unexplored_is_occupied:
ego_map[ego_map == Costmap.UNEXPLORED] = Costmap.OCCUPIED
is_colliding = np.any(footprint_mask == ego_map)
if debug:
plt.imshow(footprint_mask - 0.1 * ego_map)
plt.show()
return is_colliding
def oriented_astar(goal,
start,
occupancy_map,
footprint_masks,
outline_coords,
obstacle_values,
planning_scale=1,
delta=0.0,
epsilon=1.0,
allow_diagonal=True):
"""
Oriented Astar C++ wrapper for python. Formats input data in required format for c++ function, the calls it,
returning the path if found.
:param goal array(3)[float]: [x, y, theta] goal pose of the robot
:param start array(3)[float]: [x, y, theta] start pose of the robot
:param occupancy_map Costmap: occupancy map used for planning, data must be compatible with uint8
:param footprint_masks array(N,M,M)[int]: masks of the footprint rotated at the corresponding angles
needed for checking, i.e state[2]. N is 2 * mask_radius + 1,
the values are -1 for not footprint, 0 for footprint.
N is the dimension across angles, (M, M) is the mask shape
:param outline_coords array(N, 2)[int]: the coordinates that define the outline of the footprint. N is the number
of points that define the outline of the footprint
:param obstacle_values array(N)[uint8]: an array containing values that the collision checker should deem as an obstacle
i.e [127, 0]
:param planning_scale int: value > 1, to plan on a lower resolution than the original occupancy map resolution,
this value is round_int(desired resolution / original resolution)
:param delta float: distance in pixels to extend the goal region for solution (allow to be within delta away from goal)
TODO FIX this to be in meters
:param epsilon float: weighting for the heuristic in the A* algorithm
:param allow_diagonal bool: whether to allow diagonal movements
:return Tuple[bool, array(N, 3)[float]]: whether we were able to successfully plan to the goal,
and the most promising path to the goal node (solution if obtained)
"""
c_angles = np.array(c_get_astar_angles(), dtype=np.float32)
start_px = xy_to_rc(start, occupancy_map)
c_start = np.array(start_px[:2], dtype=np.int32)
goal_px = xy_to_rc(goal, occupancy_map)
c_goal = np.array(goal_px[:2], dtype=np.int32)
c_occupancy_map = occupancy_map.data.astype(np.uint8)
c_footprint_masks = np.logical_not(np.array(footprint_masks,
dtype=np.bool))
c_footprint_masks = [
c_footprint_mask for c_footprint_mask in c_footprint_masks
]
c_outline_coords = np.array(outline_coords, dtype=np.int32)
c_outline_coords = [
c_outline_coord for c_outline_coord in c_outline_coords
]
c_obstacle_values = np.array(obstacle_values, dtype=np.uint8)
success, path_px = c_oriented_astar(c_start, c_goal, c_occupancy_map,
c_footprint_masks, c_angles,
c_outline_coords, c_obstacle_values,
delta, epsilon, planning_scale,
allow_diagonal)
path = rc_to_xy(path_px, occupancy_map)
return success, np.vstack(([start], path))
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)
