astar.py

# Credit for this: Nicholas Swift
# as found at https://medium.com/@nicholas.w.swift/easy-a-star-pathfinding-7e6689c7f7b2
from warnings import warn
import heapq

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

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

        self.g = 0
        self.h = 0
        self.f = 0

    def __eq__(self, other):
        return self.position == other.position
    
    def __repr__(self):
      return f"{self.position} - g: {self.g} h: {self.h} f: {self.f}"

    # 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

def return_path(current_node):
    path = []
    current = current_node
    while current is not None:
        path.append(current.position)
        current = current.parent
    return path[::-1]  # Return reversed path


def astar(maze, start, end, allow_diagonal_movement = False):
    """
    Returns a list of tuples as a path from the given start to the given end in the given maze
    :param maze:
    :param start:
    :param end:
    :return:
    """

    # Create start and end node
    start_node = Node(None, start)
    start_node.g = start_node.h = start_node.f = 0
    end_node = Node(None, end)
    end_node.g = end_node.h = end_node.f = 0

    # Initialize both 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)

    # Adding a stop condition
    outer_iterations = 0
    max_iterations = (len(maze[0]) * len(maze) // 2)

    # what squares do we search
    adjacent_squares = ((0, -1), (0, 1), (-1, 0), (1, 0),)
    if allow_diagonal_movement:
        adjacent_squares = ((0, -1), (0, 1), (-1, 0), (1, 0), (-1, -1), (-1, 1), (1, -1), (1, 1),)

    # Loop until you find the end
    while len(open_list) > 0:
        outer_iterations += 1

        if outer_iterations > max_iterations:
          # if we hit this point return the path such as it is
          # it will not contain the destination
          warn("giving up on pathfinding too many iterations")
          return return_path(current_node)       
        
        # Get the current node
        current_node = heapq.heappop(open_list)
        closed_list.append(current_node)

        # Found the goal
        if current_node == end_node:
            return return_path(current_node)

        # Generate children
        children = []
        
        for new_position in adjacent_squares: # Adjacent squares

            # Get node position
            node_position = (current_node.position[0] + new_position[0], current_node.position[1] + new_position[1])

            # Make sure within range
            if node_position[0] > (len(maze) - 1) or node_position[0] < 0 or node_position[1] > (len(maze[len(maze)-1]) -1) or node_position[1] < 0:
                continue

            # Make sure walkable terrain
            if maze[node_position[0]][node_position[1]] != 0:
                continue

            # Create new node
            new_node = Node(current_node, node_position)

            # Append
            children.append(new_node)

        # Loop through children
        for child in children:
            # Child is on the closed list
            if len([closed_child for closed_child in closed_list if closed_child == child]) > 0:
                continue

            # Create the f, g, and h values
            child.g = current_node.g + 1
            child.h = ((child.position[0] - end_node.position[0]) ** 2) + ((child.position[1] - end_node.position[1]) ** 2)
            child.f = child.g + child.h

            # Child is already in the open list
            if len([open_node for open_node in open_list if child.position == open_node.position and child.g > open_node.g]) > 0:
                continue

            # Add the child to the open list
            heapq.heappush(open_list, child)

    warn("Couldn't get a path to destination")
    return None

def example(print_maze = True):

    maze = [[0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,] * 2,
            [0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,] * 2,
            [0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,] * 2,
            [0,0,0,0,0,0,0,0,0,0,1,1,1,1,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,] * 2,
            [0,0,0,1,1,0,0,1,1,1,1,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,1,0,0,0,] * 2,
            [0,0,0,1,0,0,0,1,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,1,0,1,0,] * 2,
            [0,0,0,1,0,1,1,1,1,0,1,1,0,0,1,1,1,0,0,0,1,1,1,1,1,1,1,0,0,0,] * 2,
            [0,0,0,1,0,1,0,0,0,0,0,1,0,0,0,0,1,1,0,1,0,0,0,0,0,0,1,1,1,0,] * 2,
            [0,0,0,1,0,1,1,0,1,1,0,1,1,1,0,0,0,0,0,1,0,0,1,1,1,1,1,0,0,0,] * 2,
            [0,0,0,1,0,1,0,0,0,0,0,0,0,1,1,1,1,1,1,1,0,0,0,0,1,0,1,0,1,1,] * 2,
            [0,0,0,1,0,1,0,1,1,0,1,1,1,1,0,0,1,1,1,1,1,1,1,0,1,0,1,0,0,0,] * 2,
            [0,0,0,1,0,1,0,0,0,0,1,0,0,0,0,0,0,0,0,0,1,0,0,0,1,0,1,1,1,0,] * 2,
            [0,0,0,1,0,1,1,1,1,0,1,0,0,1,1,1,0,1,1,1,1,0,1,1,1,0,1,0,0,0,] * 2,
            [0,0,0,1,0,0,0,0,1,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,1,0,1,1,] * 2,
            [0,0,0,1,0,0,0,0,0,0,0,0,1,0,0,0,0,0,1,0,0,0,0,0,0,0,1,0,0,0,] * 2,
            [1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,0,] * 2,]
    
    start = (0, 0)
    end = (len(maze)-1, len(maze[0])-1)

    path = astar(maze, start, end)

    if print_maze:
      for step in path:
        maze[step[0]][step[1]] = 2
      
      for row in maze:
        line = []
        for col in row:
          if col == 1:
            line.append("\u2588")
          elif col == 0:
            line.append(" ")
          elif col == 2:
            line.append(".")
        print("".join(line))

    print(path)

Added the fixes as pointed out by @BryceBeagle.

Additionally a stopping condition, as I need things to terminate semi-gracefully and not lock up the game loop.

The algorithm still does not work when you change maze(4,4) to 1. It'll still return a path going through (4,4)

@ryancollingwood any update on the bug mentioned by @liamhan0905 ?

PS Thanks for updated solution! I can here from your comment on the original medium post.

Howdy, I haven't updated the implementation from when I used it in a simple 2d game, https://github.com/ryancollingwood/arcade-rabbit-herder/blob/master/pathfinding/astar.py

It works well enough for my simple purposes 🐇
I have a look again this weekend

I can't duplicate the issue @liamhan0905 raised, got anymore details?

@bearcub this is my implementation of the gist originally shared by @Nicholas-Swift

See it in use at in my silly game: https://github.com/ryancollingwood/arcade-rabbit-herder/blob/master/pathfinding/astar.py

Hello,

Thank you for the gist. I think there is an error in your code.

With the following maze :

maze = [[0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
            [0, 1, 1, 0, 1, 1, 0, 1, 1, 0],
            [0, 1, 1, 0, 1, 1, 0, 1, 1, 0],
            [0, 1, 1, 0, 1, 1, 0, 1, 1, 0],
            [0, 1, 1, 0, 1, 1, 0, 1, 1, 0],
            [0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
            [0, 1, 1, 0, 1, 1, 0, 1, 1, 0],
            [0, 1, 1, 0, 1, 1, 0, 1, 1, 0],
            [0, 1, 1, 0, 1, 1, 0, 1, 1, 0],
            [0, 0, 0, 0, 0, 0, 0, 0, 0, 0]]
start = (0, 9)
end = (9, 0)

I have this result : [(0, 9), (0, 8), (0, 7), (0, 6), (1, 6), (2, 6), (3, 6), (4, 6), (5, 6), (5, 5), (5, 4), (5, 3), (6, 3), (7, 3), (8, 3), (9, 3), (9, 2), (9, 1), (9, 0)]. It walks on [1] boxes.

I think it's a weird behavior happening when the path takes upper (and/or) right directions.

Cf. https://gist.github.com/Nicholas-Swift/003e1932ef2804bebef2710527008f44#gistcomment-3069887

You need to use a priority queue for the open_list.

@mm-airmap out of interest I implemented a very simple heapque and it's fractionally quicker ;)

Hello,

Thank you for the gist. I think there is an error in your code.

With the following maze :

maze = [[0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
            [0, 1, 1, 0, 1, 1, 0, 1, 1, 0],
            [0, 1, 1, 0, 1, 1, 0, 1, 1, 0],
            [0, 1, 1, 0, 1, 1, 0, 1, 1, 0],
            [0, 1, 1, 0, 1, 1, 0, 1, 1, 0],
            [0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
            [0, 1, 1, 0, 1, 1, 0, 1, 1, 0],
            [0, 1, 1, 0, 1, 1, 0, 1, 1, 0],
            [0, 1, 1, 0, 1, 1, 0, 1, 1, 0],
            [0, 0, 0, 0, 0, 0, 0, 0, 0, 0]]
start = (0, 9)
end = (9, 0)

I have this result : [(0, 9), (0, 8), (0, 7), (0, 6), (1, 6), (2, 6), (3, 6), (4, 6), (5, 6), (5, 5), (5, 4), (5, 3), (6, 3), (7, 3), (8, 3), (9, 3), (9, 2), (9, 1), (9, 0)]. It walks on [1] boxes.

I think it's a weird behavior happening when the path takes upper (and/or) right directions.

Cf. https://gist.github.com/Nicholas-Swift/003e1932ef2804bebef2710527008f44#gistcomment-3069887

Hi @alarictabaries

I ran your map and the code solution is correct, i get the following:
Log: Path In Memory
Log: [(0, 9), (0, 8), (0, 7), (0, 6), (1, 6), (2, 6), (3, 6), (4, 6), (5, 6), (5, 5), (5, 4), (5, 3), (6, 3), (7, 3), (8, 3), (9, 3), (9, 2), (9, 1), (9, 0)]
Which looks like this:

0 | 0 | 0 | 0 | 0 | 0 | v | < | < | Start
0 | 1 | 1 | 0 | 1 | 1 | v | 1 | 1 | 0
0 | 1 | 1 | 0 | 1 | 1 | v | 1 | 1 | 0
0 | 1 | 1 | 0 | 1 | 1 | v | 1 | 1 | 0
0 | 1 | 1 | 0 | 1 | 1 | v | 1 | 1 | 0
0 | 0 | 0 | v | < | < | < | 0 | 0 | 0
0 | 1 | 1 | v | 1 | 1 | 0 | 1 | 1 | 0
0 | 1 | 1 | v | 1 | 1 | 0 | 1 | 1 | 0
0 | 1 | 1 | v | 1 | 1 | 0 | 1 | 1 | 0
F | < | < | < | 0 | 0 | 0 | 0 | 0 | 0

You should remember its not (X,Y) but rather (Row, Column) which is like (Y,X) coordinates in the output.

Im having issues with this implementation. In particular, the code seems to have issues when a node in the open list is rediscovered, but with the same g value. The function gets stuck at line 126 when I try to run it for more complex problems.

Hi, thanks a lot for your work!
I believe I have solved a couple of the mentioned issues.
First of all, the 'child is already in the open list' will not remove/update the old child, if the new g is <=; rather it will add the same child to the queue, resulting in duplicates. This is the reason why you thought you needed a max_iterations break. You can avoid using max_iterations and outer_iterations, by replacing this:

        # Child is already in the open list
        if len([open_node for open_node in open_list if child.position == open_node.position and child.g > open_node.g]) > 0:
            continue

with this (I believe it can be simplified):

        # Child is already in the open list
        index = None
        for i in range(0, len(open_list)):
            if child.position == open_list[i].position:
                index = i
                break
        
        if index:
            if child.g >= open_list[index].g:
                continue
            else:
                open_list[index] = open_list[-1]
                open_list.pop()
                if index < len(open_list):
                    heapq._siftup(open_list, index)
                    heapq._siftdown(open_list, 0, index)

The other thing is, that by not using the euclidean distance as heuristics, you will possible end up with a longer route, than needed.
However, this will increase the computational power needed, and without solving the above mentioned problem, this will max out the cpu (at least on my old laptop).
When using the code above, a long with the euclidean distance for child.h AND for child.g(!), the algorithm should work as intended:

        # Create the f, g, and h values
        child.g = current_node.g + (((child.position[0] - child.parent.position[0]) ** 2) + ((child.position[1] - child.parent.position[1]) ** 2))**0.5
        child.h = (((child.position[0] - end_node.position[0]) ** 2) + ((child.position[1] - end_node.position[1]) ** 2))**0.5
        child.f = child.g + child.h

Finally, initialization of start and end node f,h,g values are redundant, as the node class it self does this. You could however set the start_node.f and start_node.g to the euclidean distance to end_node, but this doesn't seem to make a difference really.

Hello, i'm using this for my final lecture in college. Thanks for the help!

@stabtazer thank you for your comments, I'll have a look see at them proper before the grind of new year kicks in 😎

@bismanugraha all the best for wrapping up your college lectures 🎉

@stabtazer thank you for your comments, I'll have a look see at them proper before the grind of new year kicks in 😎

@ryancollingwood,

Did you have a chance to look at what @stabtazer mentioned? Just curious what your thoughts are on it.

@frankhale @stabtazer @ryancollingwood
I simplify your code as below:

           \# Child is already in the open list
            if child in open_list: 
                idx = open_list.index(child) 
                if child.g < open_list[idx].g:
                    # update the node in the open list
                    open_list[idx].g = child.g
                    open_list[idx].f = child.f
                    open_list[idx].h = child.h
            else:
                \# Add the child to the open list
                heapq.heappush(open_list, child)

Through in function, we can fastly check whether child has been in open list. Also, I think we only need to update the g,h,f of the node which has the same position as child rather than operating the heap.
About the calculation of Euclidean distance, I agree to have a sqrt on h, otherwise, h will play the most role in wayfinding and the result will be a straight line (on weight map).