def test_start_out_of_range(self):
img = np.zeros((10, 10), dtype=bool)
path = find_path(img, (200, 0), [(3, 3), (5, 5)])
self.assertIsNone(path)
img = np.zeros((10, 10, 5), dtype=bool)
path = find_path(img, (0, 0, 13), [(3, 3, 0)])
self.assertIsNone(path)
def test_usage(self):
img = np.zeros((10, 10), dtype=bool)
img[5,:] = True
img[:, 0] = True
self.assertTrue(img[0][0])
self.assertTrue(img[5][5])
self.assertFalse(img[3][3])
self.assertIsNone(find_path(img, (3, 3), [(3, 3)]))
path = find_path(img, (0, 0), [(5, 5)])
self.assertEqual(path[0], (0, 0, 0))
self.assertEqual(path[-1], (5, 5, 0))
self.assertEqual(len(path), 5 + 1 + 5)
def test_multiple_goals(self):
img = np.zeros((10, 10), dtype=bool)
img[5,:] = True
img[:, 0] = True
path = find_path(img, (0, 0), [(3, 3), (5, 5)])
self.assertEqual(path[-1], (5, 5, 0))
self.assertEqual(len(path), 5 + 1 + 5)
def test_3d_planner(self):
mimg = np.zeros((10, 10, 5), dtype=bool)
mimg[5, :, 3] = True
mimg[:, 7, 2] = True
path = find_path(mimg, (0, 5, 3), [(7, 9, 2)])
self.assertIsNotNone(path)
def frontiers(img, start, draw=False):
"""
Find path to the best frontier (free-unknown transition)
:param img: color image with free=white, unknown=green
:param start: start pixel
:param draw: debug frontiers in pyplot
:return: extended image with drawn start and path, path
"""
size = img.shape
green = img[:, :, :] == STATE_UNKNOWN
white = img[:, :, :] == STATE_FREE
mask_right = green[:, 2:, :] & white[:, 1:-1, :]
mask_left = green[:, :-2, :] & white[:, 1:-1, :]
mask = mask_left | mask_right
z = np.zeros((size[0], 1, size[2]), dtype=np.bool)
mask = np.hstack([z, mask, z])
mask_up = green[2:, :, :] & white[1:-1, :, :]
mask_down = green[:-2, :, :] & white[1:-1, :, :]
z = np.zeros((1, size[1], size[2]), dtype=np.bool)
mask2 = mask_up | mask_down
mask = np.vstack([z, mask2, z]) | mask
z_mask_up = green[:, :, 2:] & white[:, :, 1:-1]
z_mask_down = green[:, :, :-2] & white[:, :, 1:-1]
z = np.zeros((size[0], size[1], 1), dtype=np.bool)
mask3 = z_mask_up | z_mask_down
# mask = np.concatenate([z, mask3, z], axis=2) | mask
xy = np.where(mask)
if len(xy[0]) == 0:
# there are no frontiers, i.e. no exploration path
return img, None
score = np.zeros(len(xy[0]))
for i in range(len(xy[0])):
x, y = xy[1][
i] - SLICE_OCTOMAP_SIZE // 2, SLICE_OCTOMAP_SIZE // 2 - xy[0][i]
if x > 4: # TODO - resolution and detect gate as one way only (0 is not sufficient due to impecise position
score[i] = math.hypot(x, y) * 0.03
else:
score[
i] = 0 # too cruel cut for X positive semi-space, but let's move INSIDE!
for i in range(len(xy[0])):
x, y = xy[1][
i] - SLICE_OCTOMAP_SIZE // 2, SLICE_OCTOMAP_SIZE // 2 - xy[0][i]
for j in range(len(xy[0])):
x2, y2 = xy[1][
j] - SLICE_OCTOMAP_SIZE // 2, SLICE_OCTOMAP_SIZE // 2 - xy[0][j]
dist = math.hypot(x - x2, y - y2)
if dist < 10 and score[i] > 0: # ~ 5 meters, only inside
score[i] += 1.0
if draw:
import matplotlib.pyplot as plt
line = plt.plot(xy[1] - SLICE_OCTOMAP_SIZE // 2,
SLICE_OCTOMAP_SIZE // 2 - xy[0], 'bo')
m = score > 3 * max(score) / 4
plt.plot(xy[1][m] - SLICE_OCTOMAP_SIZE // 2,
SLICE_OCTOMAP_SIZE // 2 - xy[0][m], 'ro')
plt.axes().set_aspect('equal', 'datalim')
plt.show()
drivable = white
# use 1 pixel surrounding
drivable_safe_y = drivable[2:, :] & drivable[1:-1, :] & drivable[:-2, :]
drivable_safe_xy = drivable_safe_y[:,
2:] & drivable_safe_y[:, 1:
-1] & drivable_safe_y[:, :
-2]
# add non-drivable frame to match original image size
z = np.zeros((size[0] - 2, 1, size[2]), dtype=np.bool)
tmp = np.hstack([z, drivable_safe_xy, z])
z = np.zeros((1, size[1], size[2]), dtype=np.bool)
drivable = np.vstack([z, tmp, z])
for limit_score in [3 * max(score) / 4, max(score) / 4, 0]:
# select goal positions above the limit_score
# note, that the "safe path" does not touch external boundary so it would never find path
# to frontier. As a workaround add also all 8-neighbors of frontiers.
goals = []
xy2 = np.array(xy)[:, score > limit_score]
for dx in [-1, 0, 1]:
for dy in [-1, 0, 1]:
for dz in [0]: #[-1, 0, 1]:
goals.append(xy2 + np.repeat(
np.asarray([[dy], [dx], [dz]]), xy2.shape[1], axis=1))
goals = np.hstack(goals).T[:, [1, 0, 2]]
# the path planner currently expects goals as tuple (x, y) and operation "in"
goals = set(map(tuple, goals))
path = find_path(drivable, start, goals, verbose=False)
if path is not None:
break
img[mask] = STATE_FRONTIER
if path is not None:
for x, y, z in path:
img[y][x] = STATE_PATH
else:
print('Path not found!')
return img, path