Speed up A* implementation in Python

Question

The A* algorithm is a path-finding algorithm similar to Dijkstra's algorithm, and works by visiting nodes (using a heuristic to decide which node to visit next), and comparing this node to the nodes already visited which are in the closed list.

In my implementation, the # of nodes visited / second decreases dramatically as the closed-list increases in size. While initially, the algorithm visits around 3,000 nodes/second, this decreases to less than 50 nodes/second as the closed-list grows > 10,000 nodes. The only thing that becomes computationally more expensive is the comparison of the new node to the open- and closed-lists, and storing the new node in the closed list.

Therefore, I think that I can dramatically increase performance by storing the closed-list in a more efficient way!

Here are some excerpts from my implementation. First, the Node class, which is used for the definition of all Nodes:

class Node:
    """
    A node class for A* Pathfinding
    """

    def __init__(self, parent=None, position=None):
        self.parent = parent
        self.position = position

        self.g = 0 # g = actual cost of reaching this node
        self.h = 0 # h = heuristic, used for determining which node to visit next
        self.f = 0 # f = g + h

    def __eq__(self, other):
        return self.position == other.position

    # defining less than for purposes of heap queue
    def __lt__(self, other):
        return self.f < other.f

    # defining greater than for purposes of heap queue
    def __gt__(self, other):
        return self.f > other.f

I use the heap queue for storing the open-list as I thought that this could improve speed. However, it only did so marginally (±5%).

Below is my A* implementation, condensed so that only the relevant operations are included:

def find_a_star_path(self, current_pos, target_pos):
    # Initialize start- and end-nodes with zero cost
    start_node = self.Node(None, current_pos)
    start_node.g = start_node.h = start_node.f = 0.0

    end_node = self.Node(None, target_pos)
    end_node.g = end_node.h = end_node.f = 0.0

    # Initialize open- and closed list
    open_list = []
    closed_list = []

    # Heapify the open_list and Add the start node
    heapq.heapify(open_list)
    heapq.heappush(open_list, start_node)

    # As long as there are "open" nodes, we continue A*.
    while len(open_list) > 0:
        # Find node with the lowest cost F, this is visited next
        current_node = heapq.heappop(open_list)
        closed_list.append(current_node)

        if current_node == end_node:
            # if current_node = end_node, the process is finished.

        # Some code that finds all possible next nodes from the next node
        # This next node is called child
        # child.g, child.h and child.f are calculated

        # Now check if the new node is better than another node with the same position but a different parent.

        filtered_open_nodes = (open_node for open_node in open_list if child == open_node)
        open_node = next(filtered_open_nodes, None)

        while open_node:
            if child.f > open_node.f:
                add_to_open = False
                break
            else:
                # The new node is better than the other path to this node, so remove it.
                open_list.remove(open_node)
                open_node = next(filtered_open_nodes, None)

        if add_to_open == True:
            heapq.heappush(open_list, child)

user_1639625 · Answer

There are a few problems with your code:

you never actually use the closed_list; you add nodes to it, but you never check whether the current_node is already closed
your closed_list should be a set for O(1) lookup; however, this means that you either have to add just the position or implement Node.__hash__, too
by doing open_list.remove(open_node) you may invalidate the Heap Invariant that is so carefully maintained by the algorithms in heapq, which might also lead to longer running time or worse, your A* not finding the correct result
not relevant here, but the implementation of Node.__eq__ should be consistent with __lt__ and __gt__ (comparing by a different attribute) and __hash__ (not implemented)

With this, and a few cosmetic changes, e.g. using any to check the open_list, your code might look like this:

def find_a_star_path(self, current_pos, target_pos):
    start_node = self.Node(None, current_pos)
    start_node.g = start_node.h = start_node.f = 0.0
    # no need for end node, just compare position

    open_list = [start_node] # no need to heapify list with just one element
    closed_set = set() # should be a set for O(1) "in" check

    while len(open_list) > 0:
        current_node = heapq.heappop(open_list)
        
        # checking the position here, alternatively implement Node.__hash__
        if current_node.position in closed_set:
            continue
        closed_set.add(current_node.position)

        if current_node.position == target_pos:
            # if current_node = end_node, the process is finished.

        for child in [code that finds all possible next nodes]:

            add_to_open = child.position not in closed_set and \
                not any(open_node.f <= child.f for open_node in open_list if open_node == child_node)

            if add_to_open:
                heapq.heappush(open_list, child)

But this is probably still very slow, as the linear scan of the entire open_list in each step greatly outweights the O(logn) heap operations. Usually, you can just not check whether the candidate node is already in the open_list, i.e. remove the and not any(...) check. This may lead to a few more nodes on the heap than necessary, but this might not be a problem at all. As soon as they are popped, they are discarded as by then they are already in the closed_set. (In fact you might remove the entire add_to_open check, but checking whether they are already in the closed_set here is cheap, so why not.)

If the additional (and possibly duplicated) elements on the heap are causing problems, you can replace the O(n) scan of the open_list with a dict mapping positions (or nodes, if they implement __hash__) to minimum f values, providing O(1) lookup just like the closed_set:

    closed_set = set()
    open_dict = {start_node.position: start_node.f}

and then:

    for child in [code that finds all possible next nodes]:
        pos = child.position
        add_to_open = pos not in closed_set and \
                (pos not in open_dict or open_dict[pos] > child.f)

        if add_to_open:
            heapq.heappush(open_list, child)
            open_dict[pos] = child.f

Speed up A* implementation in Python

1 Answers