/
a_star.py
executable file
·155 lines (123 loc) · 4.74 KB
/
a_star.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
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
#!/usr/bin/env python3
import sys
from src.colors import colors
from src.reader import Reader
class Node:
''' A representation of a single position '''
def __init__(self, parent=None, pos=None):
self.parent = parent
self.pos = pos
self.f = self.g = self.h = 0
self.new_pos = [(-1,-1),(0,-1),(1,-1),(-1,0),(1,0),(-1,1),(0,1),(1,1)]
def __eq__(self, other):
return self.pos == other.pos
class Maze_Solver:
''' A class for solving mazes '''
def __init__(self, f, start, goal):
self.reader = Reader(f)
self.start = tuple(map(int, start.split(',')))
self.goal = tuple(map(int, goal.split(',')))
self.expanded = []
def print_solved(self, path):
''' Print solved maze '''
if path:
tup = []
for i in range(0, len(self.reader.maze)):
for j in range(0, len(self.reader.maze)):
tup.append(i)
tup.append(j)
if tuple(tup) in self.expanded:
if tuple(tup) in path:
print(colors.yellow2 + '0' + ' ' + colors.white, end='')
else:
print(colors.blue + 'x' + ' ' + colors.white, end='')
else:
if self.reader.maze[i][j] == 1:
print(colors.green + '1' + ' ' + colors.white, end='')
else:
print(colors.white + '.' + ' ', end='')
tup = []
print('')
else:
print('Goal could not be reached!')
def in_range(self, node_pos):
''' Check to see if tile is valid '''
if node_pos[0] > (len(self.reader.maze) - 1) or node_pos[0] < 0 or node_pos[1] > (len(self.reader.maze[len(self.reader.maze) - 1]) - 1) or node_pos[1] < 0:
return False
if self.reader.maze[node_pos[0]][node_pos[1]] == 1:
return False
return True
def generate_children(self, curr):
''' Return list of possible steps '''
children = []
for newpos in curr.new_pos:
node_pos = (curr.pos[0] + newpos[0], curr.pos[1] + newpos[1])
if(self.in_range(node_pos)):
n = Node(curr.pos, node_pos)
children.append(n)
return children
# FIXME: REFACTOR FUNCTION TO EVALUATION FUNCTION
def heuristic(self, curr, child, goal_node):
''' Calculate traversal cost from g + h '''
child.g = curr.g + 1
child.h = (abs(goal_node.pos[0]-child.pos[0]) + abs(goal_node.pos[1]-child.pos[1]))
child.f = child.g + child.h
child.parent = curr
def get_lowest_node(self, open_list):
''' Return open node with the lowest cost '''
open_list.sort(key=lambda x: x.f)
a = open_list.pop(0)
return a
def is_goal(self, n, goal):
''' Check to see if we are at the goal '''
if n == goal:
path = []
while n is not None:
path.append(n.pos)
n = n.parent
path.reverse()
return path
def a_star(self):
''' Perform A* search for the goal '''
# initialization
start_node = Node(None, self.start)
goal_node = Node(None, self.goal)
open_list = []
closed_list = []
open_list.append(start_node)
curr_succ_cost = 0
# main search loop
while open_list:
# pop lowest node from open list
n = self.get_lowest_node(open_list)
# return path if goal has been reached
path = self.is_goal(n, goal_node)
if path:
return path
# generate children of node n
children = self.generate_children(n)
# compare each child
for child in children:
# save expanded node
self.expanded.append(child.pos)
# set child variables f, g, h
self.heuristic(n, child, goal_node)
if child in open_list:
if child.g > curr_succ_cost:
continue
elif child in closed_list:
if child.g > curr_succ_cost:
continue
else:
open_list.append(child)
else:
open_list.append(child)
closed_list.append(n)
def main():
# Create a Solver that reads in the maze
ms = Maze_Solver(sys.argv[1], sys.argv[2], sys.argv[3])
# Return path if one exists
path = ms.a_star()
# Print path
ms.print_solved(path)
main()