forked from hjweide/pyastar2d
-
Notifications
You must be signed in to change notification settings - Fork 1
/
examples.py
53 lines (39 loc) · 1.42 KB
/
examples.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
import cv2
import numpy as np
import pyastar
from time import time
from os.path import basename, join, splitext
# input/output files
#MAZE_FPATH = join('mazes', 'maze_small.png')
MAZE_FPATH = join('mazes', 'maze_large.png')
OUTP_FPATH = join('solns', '%s_soln.png' % splitext(basename(MAZE_FPATH))[0])
def main():
maze = cv2.imread(MAZE_FPATH)
if maze is None:
print('no file found: %s' % (MAZE_FPATH))
return
else:
print('loaded maze of shape %r' % (maze.shape[0:2],))
grid = cv2.cvtColor(maze, cv2.COLOR_BGR2GRAY).astype(np.float32)
grid[grid == 0] = 1.0#np.inf
grid[grid == 255] = 0.0
# start is the first white block in the top row
start_j, = np.where(grid[0, :] == 0)
start = np.array([0, start_j[0]])
# end is the first white block in the final column
end_i, = np.where(grid[:, -1] == 0)
end = np.array([end_i[0], grid.shape[0] - 1])
t0 = time()
# set allow_diagonal=True to enable 8-connectivity
path = pyastar.weighted_astar_path(grid, start, end, allow_diagonal=True, wscale=4, niters=3)
dur = time() - t0
if path.shape[0] > 0:
print('found path of length %d in %.6fs' % (path.shape[0], dur))
maze[path[:, 0], path[:, 1]] = (0, 0, 255)
print('plotting path to %s' % (OUTP_FPATH))
cv2.imwrite(OUTP_FPATH, maze)
else:
print('no path found')
print('done')
if __name__ == '__main__':
main()