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