-
Notifications
You must be signed in to change notification settings - Fork 0
/
pathfinder.py
206 lines (157 loc) · 6.93 KB
/
pathfinder.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
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
import logging
import random # for test
from priorityqueueset import PriorityQueueSet
class PacworldPathFinder(object):
"""singleton instance provider of a PathFinder specific to the Pacworld World grid"""
instance = None
@staticmethod
def getInstance():
return PacworldPathFinder.instance
@staticmethod
def setInstance(_instance):
PacworldPathFinder.instance = _instance
class PathFinder(object):
""" Computes a path in a graph using the A* algorithm.
Initialize the object and then repeatedly compute_path to
get the path between a start point and an end point.
The points on a graph are required to be hashable and
comparable with __eq__. Other than that, they may be
represented as you wish, as long as the functions
supplied to the constructor know how to handle them.
"""
def __init__(self, successors, move_cost, heuristic_to_goal):
""" Create a new PathFinder. Provided with several
functions that represent your graph and the costs of
moving through it.
successors:
A function that receives a point as a single
argument and returns a list of "successor" points,
the points on the graph that can be reached from
the given point.
move_cost:
A function that receives two points as arguments
and returns the numeric cost of moving from the
first to the second.
heuristic_to_goal:
A function that receives a point and a goal point,
and returns the numeric heuristic estimation of
the cost of reaching the goal from the point.
"""
self.successors = successors
self.move_cost = move_cost
self.heuristic_to_goal = heuristic_to_goal
def compute_path(self, start, goal):
""" Compute the path between the 'start' point and the
'goal' point.
The path is returned as an iterator to the points,
including the start and goal points themselves.
If no path was found, an empty list is returned.
"""
#
# Implementation of the A* algorithm.
#
closed_set = {}
start_node = self._Node(start)
start_node.g_cost = 0
start_node.f_cost = self._compute_f_cost(start_node, goal)
open_set = PriorityQueueSet()
open_set.add(start_node)
while len(open_set) > 0:
# Remove and get the node with the lowest f_score from
# the open set
#
curr_node = open_set.pop_smallest()
#logging.debug("Checking successors for node: {0}".format(curr_node))
if curr_node.coord == goal:
return self._reconstruct_path(curr_node)
closed_set[curr_node] = curr_node
for succ_coord in self.successors(curr_node.coord):
succ_node = self._Node(succ_coord)
succ_node.g_cost = self._compute_g_cost(curr_node, succ_node)
succ_node.f_cost = self._compute_f_cost(succ_node, goal)
if succ_node in closed_set:
continue
if open_set.add(succ_node):
succ_node.pred = curr_node
return []
########################## PRIVATE ##########################
def _compute_g_cost(self, from_node, to_node):
return (from_node.g_cost +
self.move_cost(from_node.coord, to_node.coord))
def _compute_f_cost(self, node, goal):
return node.g_cost + self._cost_to_goal(node, goal)
def _cost_to_goal(self, node, goal):
return self.heuristic_to_goal(node.coord, goal)
def _reconstruct_path(self, node):
""" Reconstructs the path to the node from the start node
(for which .pred is None)
"""
pth = [node.coord]
n = node
while n.pred:
n = n.pred
pth.append(n.coord)
return reversed(pth)
class _Node(object):
""" Used to represent a node on the searched graph during
the A* search.
Each Node has its coordinate (the point it represents),
a g_cost (the cumulative cost of reaching the point
from the start point), a f_cost (the estimated cost
from the start to the goal through this point) and
a predecessor Node (for path construction).
The Node is meant to be used inside PriorityQueueSet,
so it implements equality and hashinig (based on the
coordinate, which is assumed to be unique) and
comparison (based on f_cost) for sorting by cost.
"""
def __init__(self, coord, g_cost=None, f_cost=None, pred=None):
self.coord = coord
self.g_cost = g_cost
self.f_cost = f_cost
self.pred = pred
def __eq__(self, other):
return self.coord == other.coord
def __lt__(self, other):
return self.f_cost < other.f_cost
def __le__(self, other):
return self.f_cost <= other.f_cost
def __gt__(self, other):
return self.f_cost > other.f_cost
def __ge__(self, other):
return self.f_cost >= other.f_cost
def __hash__(self):
return hash(self.coord)
def __str__(self):
return 'N(%s) -> g: %s, f: %s' % (self.coord, self.g_cost, self.f_cost)
def __repr__(self):
return self.__str__()
if __name__ == "__main__":
from world import World # for testing
logging.basicConfig(format='%(asctime)-15s:%(levelname)s:%(filename)s#%(funcName)s(): %(message)s', level=logging.DEBUG, filename='debug.log')
crazySeed = random.randint(0, 65535)
#crazySeed = 38849
print("USING RANDOM SEED: {0}".format(crazySeed))
random.seed(crazySeed)
#gridDisplaySize = (20, 10)
gridDisplaySize = (40, 20)
#gridDisplaySize = (100, 35)
# Create the world, passing through the display size
theworld = World(gridDisplaySize)
start = 0, 0
goal = gridDisplaySize[1]-1, gridDisplaySize[0]-1
pf = PathFinder(theworld.successors, theworld.move_cost, theworld.move_cost)
import time
t = time.clock()
logging.debug("Computing path for world map:\n"+theworld.to_s())
path = list(pf.compute_path(start, goal))
print("Elapsed: %s" % (time.clock() - t))
if(path):
print("Path from {0} to {1} is: {2}".format(start,goal,path))
# show the final product
print("world map with path:")
print(theworld.to_s(path))
else:
print("Path could not be found from {0} to {1}".format(start,goal))
print("world map is:")
print(theworld.to_s())