def get_path(self, start, end, board, cost_estimate=get_distance):
t0 = time.time()
explored = set()
previous = {}
previous[start] = None
moves = {}
moves[start] = 0
frontier = PriorityQueue()
frontier.insert(start, cost_estimate(start, end))
if VERBOSE_ASTAR: print 'get_path start, end:', start, end
while not frontier.is_empty():
if (time.time() - t0 > PATHFINDER_TIMEOUT):
print 'PATHFINDING TIMEOUT: Averting disconnect...'
print ' get_path: Probably could not find a valid path from', start, 'to', end
return [start, start]
if VERBOSE_ASTAR: print 'get_path frontier:', frontier
current = frontier.remove()
explored.add(current)
if VERBOSE_ASTAR: print 'get_path explored set', explored
if VERBOSE_ASTAR: print 'get_path current pos:', current
if (current == end):
if VERBOSE_ASTAR: print 'Found end loc'
break
else:
neighbors = get_neighboring_locs(current, board)
if VERBOSE_ASTAR: print 'get_path neighbors:', neighbors
for n in neighbors:
if n not in explored and (board.passable(n)
or n in (start, end)):
moves[n] = moves[current] + MOVE_COST
frontier.insert(n, cost_estimate(n, end) + moves[n])
previous[n] = current
# found goal, now reconstruct path
i = end
path = [i]
while i != start:
if (i in previous):
path.append(previous[i])
i = previous[i]
else:
print 'get_path error: probably could not find a valid path from', start, 'to', end
return [start, start] # return something valid
path.reverse()
return path
def get_path(self, start, end, board, cost_estimate=get_distance):
t0 = time.time()
explored = set()
previous = {}
previous[start] = None
moves = {}
moves[start] = 0
frontier = PriorityQueue()
frontier.insert(start, cost_estimate(start, end))
if VERBOSE_ASTAR: print 'get_path start, end:', start, end
while not frontier.is_empty():
if (time.time() - t0 > PATHFINDER_TIMEOUT):
print 'PATHFINDING TIMEOUT: Averting disconnect...'
print ' get_path: Probably could not find a valid path from', start, 'to', end
return [start, start]
if VERBOSE_ASTAR: print 'get_path frontier:', frontier
current = frontier.remove()
explored.add(current)
if VERBOSE_ASTAR: print 'get_path explored set', explored
if VERBOSE_ASTAR: print 'get_path current pos:', current
if (current == end):
if VERBOSE_ASTAR: print 'Found end loc'
break
else:
neighbors = get_neighboring_locs(current, board)
if VERBOSE_ASTAR: print 'get_path neighbors:', neighbors
for n in neighbors:
if n not in explored and (board.passable(n) or n in (start, end)):
moves[n] = moves[current] + MOVE_COST
frontier.insert(n, cost_estimate(n, end) + moves[n])
previous[n] = current
# found goal, now reconstruct path
i = end
path = [i]
while i != start:
if (i in previous):
path.append(previous[i])
i = previous[i]
else:
print 'get_path error: probably could not find a valid path from', start, 'to', end
return [start, start] # return something valid
path.reverse()
return path
def choose_best(self, locs):
best = None
q = PriorityQueue()
print 'choose best, locs:', locs
if locs != None:
for loc in locs:
q.insert(loc, -self.score_loc(loc)) # by highest score
best = q.remove()
print 'choose best, best:', best
return best
def astar(grid, start, goal):
'''Return a path found by A* alogirhm
and the number of steps it takes to find it.
arguments:
grid - A nested list with datatype int. 0 represents free space while 1 is obstacle.
e.g. a 3x3 2D map: [[0, 0, 0], [0, 1, 0], [0, 0, 0]]
start - The start node in the map. e.g. [0, 0]
goal - The goal node in the map. e.g. [2, 2]
return:
path - A nested list that represents coordinates of each step (including start and goal node),
with data type int. e.g. [[0, 0], [0, 1], [0, 2], [1, 2], [2, 2]]
steps - Number of steps it takes to find the final solution,
i.e. the number of nodes visited before finding a path (including start and goal node)
>>> from main import load_map
>>> grid, start, goal = load_map('test_map.csv')
>>> astar_path, astar_steps = astar(grid, start, goal)
It takes 7 steps to find a path using A*
>>> astar_path
[[0, 0], [1, 0], [2, 0], [3, 0], [3, 1]]
'''
debug_draw = False
path = []
steps = 0
found = False
map = map2d(grid)
frontier = PriorityQueue()
frontier.put(start, map.get_manhattan_distance(start, goal))
came_from = {}
came_from[tuple(start)] = {
'from': None,
'cost': 0
}
while not frontier.is_empty():
(curr_cost, current) = frontier.get()
frontier.remove()
if tuple(goal) in came_from.keys():
found = True
break
for neighbor in map.get_neighbors(current):
if neighbor is None or map.get_value(neighbor) == 1:
continue
neighbor_cost = curr_cost - map.get_manhattan_distance(current, goal) + \
map.get_manhattan_distance(current, neighbor) + \
map.get_manhattan_distance(neighbor, goal)
if tuple(neighbor) not in came_from or \
neighbor_cost < came_from.get(tuple(neighbor)).get('cost'):
frontier.put(neighbor, neighbor_cost)
came_from[tuple(neighbor)] = {
'from': current,
'cost': neighbor_cost
}
if debug_draw: map.draw_path(start = start, goal = goal, path = path, came_from = came_from)
# found = True
steps = len(came_from) - 1
curr_point = goal
while curr_point != start:
path.append(curr_point)
curr_point = came_from.get(tuple(curr_point)).get('from')
path.append(start)
path.reverse()
if found:
print(f"It takes {steps} steps to find a path using A*")
else:
print("No path found")
return path, steps
class AStar():
'''
Properties:
public:
- world: 2D array of Nodes
internal:
- size: (width, height) tuple of world
- open: Nodes queue to evaluate (heap-based priority queue)
'''
#----------------------------------------------------------------------
def __init__(self, world):
self.world = world
self.size = (len(world), len(world[0]))
# self.open = SortedList()
self.open = PriorityQueue()
self.openValue = 1
self.closedValue = 2
#----------------------------------------------------------------------
def initSearch(self, start, goal, obstacles):
''' first, check we can achieve the goal'''
if goal.type in obstacles:
return False
''' clear open list and setup new open/close value state to avoid the clearing of a closed list'''
self.open.clear()
self.openValue += 2
self.closedValue += 2
''' then init search variables'''
self.start = start
self.goal = goal
self.obstacles = obstacles
self.start.cost = 0
self.addToOpen(self.start)
self.goal.parent = None
return True
#----------------------------------------------------------------------
def search(self):
while not self.openIsEmpty():
current = self.popFromOpen()
if current == self.goal:
break
self.removeFromOpen(current)
self.addToClosed(current)
''' generator passes : look at the 8 neighbours around the current node from open'''
for (di, dj) in [(-1,-1), (-1,0), (-1,1), (0,-1), (0,1), (1,-1), (1,0), (1,1)]:
neighbour = self.getNode(current.i + di, current.j + dj)
if (not neighbour) or (neighbour.type in self.obstacles):
continue
'''the cost to get to this node is the current cost plus the movement
cost to reach this node. Note that the heuristic value is only used
in the open list'''
nextStepCost = current.cost + self.getNeighbourCost(current, neighbour)
'''if the new cost we've determined for this node is lower than
it has been previously makes sure the node has not been
determined that there might have been a better path to get to
this node, so it needs to be re-evaluated'''
if nextStepCost < neighbour.cost and (self.inOpenList(neighbour) or self.inClosedList(neighbour)):
self.invalidateState(neighbour)
'''if the node hasn't already been processed and discarded then
step (i.e. to the open list)'''
if (not self.inOpenList(neighbour)) and (not self.inClosedList(neighbour)):
neighbour.cost = nextStepCost
neighbour.heuristic = self.getHeuristicCost(neighbour, self.goal)
neighbour.parent = current
self.addToOpen(neighbour)
''' exit with None = path not yet found'''
yield None
'''since we've run out of search
there was no path. Just return'''
if self.goal.parent is None:
return
'''At this point we've definitely found a path so we can uses the parent
references of the nodes to find out way from the target location back
to the start recording the nodes on the way.'''
path = []
goal = self.goal
while goal is not self.start:
path.insert(0, (goal.i, goal.j))
goal = goal.parent
''' done, exit with path'''
yield path
#-----------------------------------------------------------------------------
def getNode(self, i, j):
if i >=0 and i < self.size[0] and j >= 0 and j < self.size[1]:
return self.world[i][j]
else:
return None
#----------------------------------------------------------------------
def getNeighbourCost(self, n1, n2):
return (abs(n2.i - n1.i) + abs(n2.j - n1.j))
#----------------------------------------------------------------------
def getHeuristicCost(self, n1, n2):
return (abs(n2.i - n1.i) + abs(n2.j - n1.j))
#----------------------------------------------------------------------
def invalidateState(self, node):
node.state = 0
#----------------------------------------------------------------------
def popFromOpen(self):
# return self.open.first()
return self.open.pop()
#----------------------------------------------------------------------
def addToOpen(self, node):
# self.open.add(node)
self.open.insert(node)
node.state = self.openValue
#----------------------------------------------------------------------
def inOpenList(self, node):
return node.state is self.openValue
#----------------------------------------------------------------------
def removeFromOpen(self, node):
# self.open.remove(node)
self.open.remove(node)
node.state = 0
#----------------------------------------------------------------------
def openIsEmpty(self):
# return not self.open.size()
return self.open.isEmpty()
#----------------------------------------------------------------------
def addToClosed(self, node):
node.state = self.closedValue
#----------------------------------------------------------------------
def inClosedList(self, node):
return node.state is self.closedValue
class AStar():
'''
Properties:
public:
- world: 2D array of Nodes
internal:
- size: (width, height) tuple of world
- open: Nodes queue to evaluate (heap-based priority queue)
'''
#----------------------------------------------------------------------
def __init__(self, world):
self.world = world
self.size = (len(world), len(world[0]))
# self.open = SortedList()
self.open = PriorityQueue()
self.openValue = 1
self.closedValue = 2
#----------------------------------------------------------------------
def initSearch(self, start, goal, obstacles):
''' first, check we can achieve the goal'''
if goal.type in obstacles:
return False
''' clear open list and setup new open/close value state to avoid the clearing of a closed list'''
self.open.clear()
self.openValue += 2
self.closedValue += 2
''' then init search variables'''
self.start = start
self.goal = goal
self.obstacles = obstacles
self.start.cost = 0
self.addToOpen(self.start)
self.goal.parent = None
return True
#----------------------------------------------------------------------
def search(self):
while not self.openIsEmpty():
current = self.popFromOpen()
if current == self.goal:
break
self.removeFromOpen(current)
self.addToClosed(current)
''' generator passes : look at the 8 neighbours around the current node from open'''
for (di, dj) in [(-1, -1), (-1, 0), (-1, 1), (0, -1), (0, 1),
(1, -1), (1, 0), (1, 1)]:
neighbour = self.getNode(current.i + di, current.j + dj)
if (not neighbour) or (neighbour.type in self.obstacles):
continue
'''the cost to get to this node is the current cost plus the movement
cost to reach this node. Note that the heuristic value is only used
in the open list'''
nextStepCost = current.cost + self.getNeighbourCost(
current, neighbour)
'''if the new cost we've determined for this node is lower than
it has been previously makes sure the node has not been
determined that there might have been a better path to get to
this node, so it needs to be re-evaluated'''
if nextStepCost < neighbour.cost and (
self.inOpenList(neighbour)
or self.inClosedList(neighbour)):
self.invalidateState(neighbour)
'''if the node hasn't already been processed and discarded then
step (i.e. to the open list)'''
if (not self.inOpenList(neighbour)) and (
not self.inClosedList(neighbour)):
neighbour.cost = nextStepCost
neighbour.heuristic = self.getHeuristicCost(
neighbour, self.goal)
neighbour.parent = current
self.addToOpen(neighbour)
''' exit with None = path not yet found'''
yield None
'''since we've run out of search
there was no path. Just return'''
if self.goal.parent is None:
return
'''At this point we've definitely found a path so we can uses the parent
references of the nodes to find out way from the target location back
to the start recording the nodes on the way.'''
path = []
goal = self.goal
while goal is not self.start:
path.insert(0, (goal.i, goal.j))
goal = goal.parent
''' done, exit with path'''
yield path
#-----------------------------------------------------------------------------
def getNode(self, i, j):
if i >= 0 and i < self.size[0] and j >= 0 and j < self.size[1]:
return self.world[i][j]
else:
return None
#----------------------------------------------------------------------
def getNeighbourCost(self, n1, n2):
return (abs(n2.i - n1.i) + abs(n2.j - n1.j))
#----------------------------------------------------------------------
def getHeuristicCost(self, n1, n2):
return (abs(n2.i - n1.i) + abs(n2.j - n1.j))
#----------------------------------------------------------------------
def invalidateState(self, node):
node.state = 0
#----------------------------------------------------------------------
def popFromOpen(self):
# return self.open.first()
return self.open.pop()
#----------------------------------------------------------------------
def addToOpen(self, node):
# self.open.add(node)
self.open.insert(node)
node.state = self.openValue
#----------------------------------------------------------------------
def inOpenList(self, node):
return node.state is self.openValue
#----------------------------------------------------------------------
def removeFromOpen(self, node):
# self.open.remove(node)
self.open.remove(node)
node.state = 0
#----------------------------------------------------------------------
def openIsEmpty(self):
# return not self.open.size()
return self.open.isEmpty()
#----------------------------------------------------------------------
def addToClosed(self, node):
node.state = self.closedValue
#----------------------------------------------------------------------
def inClosedList(self, node):
return node.state is self.closedValue
def find(mapdata, width, height, start, end):
""" mapdata is a one-dimensional list of values, start and end are vectors of size 2 """
# WRITE THIS FUNCTION
open = PriorityQueue()
closed = []
curTile = MapTile(start, None, None, None, None)
print(width, height, start, end)
while curTile.coords != end:
if onMap(curTile.coords, width, height):
n = north(curTile.coords)
nter = terraintype(mapdata, width, height, n)
ntile = MapTile(n, LAT_COST, mandistance(n, end), nter, curTile)
if nter and (ntile not in closed):
print(ntile)
open.insert(ntile)
s = south(curTile.coords)
ster = terraintype(mapdata, width, height, s)
stile = MapTile(s, LAT_COST, mandistance(s, end), ster, curTile)
if ster and (stile not in closed):
print(stile)
open.insert(stile)
e = east(curTile.coords)
eter = terraintype(mapdata, width, height, e)
etile = MapTile(e, LAT_COST, mandistance(e, end), eter, curTile)
if eter and (etile not in closed):
print(etile)
open.insert(etile)
w = west(curTile.coords)
wter = terraintype(mapdata, width, height, w)
wtile = MapTile(w, LAT_COST, mandistance(w, end), wter, curTile)
if wter and (wtile not in closed):
print(wtile)
open.insert(wtile)
nw = northwest(curTile.coords)
nwter = terraintype(mapdata, width, height, nw)
nwtile = MapTile(nw, DIAG_COST, mandistance(nw, end), nwter, curTile)
if nwter and (nwtile not in closed):
print(nwtile)
open.insert(nwtile)
ne = northeast(curTile.coords)
neter = terraintype(mapdata, width, height, ne)
netile = MapTile(ne, DIAG_COST, mandistance(ne, end), neter, curTile)
if neter and (netile not in closed):
print(netile)
open.insert(netile)
sw = southwest(curTile.coords)
swter = terraintype(mapdata, width, height, sw)
swtile = MapTile(sw, DIAG_COST, mandistance(sw, end), swter, curTile)
if swter and (swtile not in closed):
print(swtile)
open.insert(swtile)
se = southeast(curTile.coords)
seter = terraintype(mapdata, width, height, se)
setile = MapTile(se, DIAG_COST, mandistance(se, end), seter, curTile)
if seter and (setile not in closed):
print(setile)
open.insert(setile)
closed.append(curTile)
print(open)
curTile = open.remove()
path = []
if curTile.coords == end:
while curTile.parent is not None:
path.append(curTile.parent)
curTile = curTile.parent
print(path)
