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()
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 []
def best_path(self, posA, posB):
results = []
closedList = []
openList = PriorityQueueSet()
startNode = Node(posA)
endNode = Node(posB)
# if start node or end node is impassable then this can not
# happen
if self.impassablePred(posA) or self.impassablePred(posB):
return []
startNode.costFromStart = self.costFunc(posA)
startNode.totalCost = startNode.costFromStart
reachedEndNode = False
pathEndNode = None
# add start node to the open list
openList.add(startNode, (startNode.totalCost, startNode.costFromStart))
while len(openList) > 0:
#logging.debug('------------------------------')
#logging.debug('open: %s' % str(openList))
#logging.debug('closed: %s' % str(closedList))
node = openList.pop()
# check if rNode is the end point, if so then we are done
if node == endNode:
reachedEndNode = True
pathEndNode = node
break
closedList.append(node)
#logging.debug('node: %s' % str(node))
adjacencies = self.adjacenciesFunc(node.elem)
for r in adjacencies:
# first check if impassable, if so then skip
if self.impassablePred(r):
continue
h = self.heuristicCostFunc(r, posB)
g = node.costFromStart + self.costFunc(r)
f = g + h
rNode = Node(r, parent=node, totalCost=f, costFromStart=g)
# check if rNode is already in the closed list and if
# so then ignore it
if rNode in closedList:
continue
# check if rNode is already in the open list and if so
# then if it's G cost is better or the same then skip
# to next node
if rNode in openList:
olNodeCost = openList.get(rNode)[1]
if olNodeCost <= rNode.costFromStart:
continue
# add rNode to the open list
openList.add(rNode, (rNode.totalCost, rNode.costFromStart))
# backtrack and copy results into the results buffer
if reachedEndNode:
n = pathEndNode
while n:
results.append(n.elem)
n = n.parent
# return it in the forward order
results.reverse()
return results