Ejemplo n.º 1
0
    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 []
Ejemplo n.º 2
0
    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