A* algorithm in python having trouble with making the final path

Question:

Full now working Code:
https://docs.google.com/document/d/1j1BLe4L735nmXWR0Vnj3TeHumzI5x7Ea5yWJSc5j-8w/edit

I tried to code the A* pathfinding algorithm and I got it to expand in the right way using the g and h costs.

It looked really cool, however when I tried finding the final path by using recursion starting on the end node it was wrong in most cases.

I want to know if this is a problem with A* or my code as when I found someone else’s code for the algorithm it has the same problem.

I did this using pygame in python.

Here is the relevant part of my code (most of pygame interaction excluded):

BLACK = (0, 0, 0)
WHITE = (255, 255, 255)
PURPLE = (238, 130, 238)
pygame.init()
width = 1200
hight = 900
startNode = None
endNode = None
SCREEN = pygame.display.set_mode((width, hight), pygame.DOUBLEBUF)

class node:

   def __init__(self, pos, h_cost, g_cost, f_cost, Color):
       self.x = pos[0]
       self.y = pos[1]
       self.parent = None
       self.f_cost = f_cost
       self.g_cost = g_cost
       self.h_cost = h_cost
       self.pos = pos
       self.Color = Color

   def TraceBack(self):
       if self.parent != None:
           self.parent.Color = "BLUE"
           self.parent.TraceBack()

   def calG_cost(self, startNode):
       self.g_cost = math.sqrt((self.x - startNode.x) ** 2 + abs(self.y - startNode.y) ** 2)

   def calH_cost(self,endNode):
       self.h_cost = math.sqrt((self.x - endNode.x) ** 2 + abs(self.y - endNode.y) ** 2)

   def calF_cost(self,endNode,startNode):
       self.calG_cost(startNode)
       self.calH_cost(endNode)
       self.f_cost = self.g_cost + self.h_cost

grid = []

def checkIfDone(endNode):
   x = endNode.x
   y = endNode.y
   if grid[x + 1][y].Color == "PURPLE":
       return True

   if grid[x + 1][y + 1].Color == "PURPLE":
       return True
   if grid[x + 1][y - 1].Color == "PURPLE":
       return True
   if  grid[x - 1][y - 1].Color == "PURPLE":
       return True

   if grid[x - 1][y].Color == "PURPLE":
       return True

   if grid[x - 1][y + 1].Color == "PURPLE":
       return True

   if grid[x][y + 1].Color == "PURPLE":
       return True

   if grid[x][y - 1].Color == "PURPLE":
       return True
   return False



def findLowestFCost():
   lowest = math.inf
   lowestNode = None
   for row in grid:
       for node in row:
           if node.Color == "GREEN" and node.f_cost < lowest:
               lowest = node.f_cost
               lowestNode = node

   return lowestNode

def GenerateGrid():
   for x in range(100):
       grid.append([])
       for y in range(100):
           grid[x].append("")

   for y in range(len(grid[0]) ):
       for x in range(len(grid) ):
           grid[x][y] = node((x, y), 0, 0, 0, "")


def checkFound(parent):
   x = parent.x
   y = parent.y
   try:
       if grid[x + 1][y].Color == "RED": return True

       if grid[x + 1][y + 1].Color == "RED": return True

       if grid[x + 1][y - 1].Color == "RED": return True

       if grid[x - 1][y - 1].Color == "RED": return True

       if grid[x - 1][y].Color == "RED": return True

       if grid[x - 1][y + 1].Color == "RED": return True

       if grid[x][y + 1].Color == "RED": return True

       if grid[x][y - 1].Color == "RED": return True

   except:
       pass


def expandParent(parent):
   x = parent.x
   y = parent.y
   if grid[x + 1][y].Color == "" or grid[x + 1][y].Color == "GREEN":
       grid[x + 1][y].Color = "GREEN"

       if grid[x + 1][y].parent == None:
           grid[x + 1][y].parent = parent

       elif parent.f_cost < grid[x + 1][y].parent.f_cost:
           grid[x + 1][y].parent = parent

   if grid[x + 1][y + 1].Color == "" or grid[x + 1][y + 1].Color == "GREEN":
       grid[x + 1][y + 1].Color = "GREEN"

       if grid[x + 1][y + 1].parent == None:
           grid[x + 1][y + 1].parent = parent

       elif parent.f_cost < grid[x + 1][y + 1].parent.f_cost:
           grid[x + 1][y + 1].parent = parent

   if grid[x + 1][y - 1].Color == "" or grid[x + 1][y - 1].Color == "GREEN":
       grid[x + 1][y - 1].Color = "GREEN"

       if grid[x + 1][y - 1].parent == None:
           grid[x + 1][y - 1].parent = parent

       elif parent.f_cost < grid[x + 1][y - 1].parent.f_cost:
           grid[x + 1][y - 1].parent = parent

   if grid[x - 1][y - 1].Color == "" or grid[x - 1][y - 1].Color == "GREEN":
       grid[x - 1][y - 1].Color = "GREEN"

       if grid[x - 1][y - 1].parent == None:
           grid[x - 1][y - 1].parent = parent

       elif parent.f_cost < grid[x - 1][y - 1].parent.f_cost:
           grid[x - 1][y - 1].parent = parent

   if grid[x - 1][y].Color == "" or grid[x - 1][y].Color == "GREEN":
       grid[x - 1][y].Color = "GREEN"

       if grid[x - 1][y].parent == None:
           grid[x - 1][y].parent = parent

       elif parent.f_cost < grid[x - 1][y].parent.f_cost:
           grid[x - 1][y].parent = parent

   if grid[x - 1][y + 1].Color == "" or grid[x - 1][y + 1].Color == "GREEN":
       grid[x - 1][y + 1].Color = "GREEN"

       if grid[x - 1][y + 1].parent == None:
           grid[x - 1][y + 1].parent = parent

       elif parent.f_cost < grid[x - 1][y + 1].parent.f_cost:
           grid[x - 1][y + 1].parent = parent

   if grid[x][y + 1].Color == "" or grid[x][y + 1].Color == "GREEN":
       grid[x][y + 1].Color = "GREEN"

       if grid[x][y + 1].parent == None:
           grid[x][y + 1].parent = parent

       elif parent.f_cost < grid[x][y + 1].parent.f_cost:
           grid[x][y + 1].parent = parent

   if grid[x][y - 1].Color == "" or grid[x][y - 1].Color == "GREEN":
       grid[x][y - 1].Color = "GREEN"

       if grid[x][y - 1].parent == None:
           grid[x][y - 1].parent = parent

       elif parent.f_cost < grid[x][y - 1].parent.f_cost:
           grid[x][y - 1].parent = parent

   parent.Color = "PURPLE"

Red is the end node, Blue is the final path, purple is the parent node. I can’t see the start node as it gets overwritten. I will fix this at some point:

image of maze

Worked better here:

image of better maze

Asked By: Tamer Vassib

||

Answers:

One of the main issues is how you calculate the -cost:

   def calG_cost(self, startNode):
       self.g_cost = math.sqrt((self.x - startNode.x) ** 2 + abs(self.y - startNode.y) ** 2)

This calculates a distance from the starting node to the current node "as the crow flies", but should reflect the actual traversed path. We cannot generally know that from just the start node and the current node. This needs to be accumulated incrementally as nodes are expanded, and the next step is added to the cost.

Another issue is that findLowestFCost is not efficient: it needs to visit every cell in the grid. The A* algorithm is designed for use with a priority queue for this purpose.

expandParent has very similar code that gets repeated several times. This can be done in a better way. As you are already using a Node class, why not register the neighbors of each node in a neighbors attribute. That way you can iterate that neighbors collection in a loop.

This is what I did in the following solution. Note that it is text-based, there is no integration with pygame.

from heapq import heappop, heappush

DIAGDIST = 2**0.5
# Define meaning of letters
WALL = "#"
START = "y"
END = "r"
NEW = "."
QUEUED = "?"
VISITED = ","
ONPATH = "B"

class Node:
    def __init__(self, pos, *neighbors):
        self.x, self.y = pos
        self.pos = pos
        self.color = NEW
        self.parent = None
        self.f_cost = self.g_cost = float("inf")
        # Define neighbor bi-directional relations 
        self.neighbors = set()
        for neighbor in neighbors:
            if neighbor:
                self.neighbors.add(neighbor)
                neighbor.neighbors.add(self)

    def minDistance(self, other):
        # minimum distance with legal steps when there would be no walls
        dx = abs(self.x - other.x)
        dy = abs(self.y - other.y)
        diag = min(dx, dy)
        straight = max(dx, dy) - diag
        return diag * DIAGDIST + straight

    # Make Node instances comparable; for use by a heap
    def __lt__(self, other):
        return (self.f_cost - other.f_cost or self.x - other.x or self.y - other.y) < 0
    
    def traceBack(self):
        node = self
        while node:
            node.color = ONPATH
            node = node.parent



class Maze:
    def __init__(self, pattern):
        pattern = pattern.replace(" ", "")  # Remove intermediate spaces
        lines = pattern.strip().splitlines()
        row = [None] * len(lines[0])
        self.startNode = self.endNode = None
        self.grid = []
        for x, line in enumerate(lines):
            left = None
            # Create a row of Nodes that are immediately connected to previously created neighbors
            #   Don't create Nodes for Walls.
            row = [left := Node((x, y), left, *row[max(0, y-1):y+2]) if ch != WALL else None 
                   for y, ch in enumerate(line)]
            self.grid.append(row)
            if not self.startNode:
                self.startNode = next((node for node, ch in zip(row, line) if ch == START), None)
            if not self.endNode:
                self.endNode = next((node for node, ch in zip(row, line) if ch == END), None)
    
    def __str__(self):
        return "n".join([
            " ".join(node.color if node else WALL for node in row)
            for row in self.grid
        ])


    def aStarSearch(self):
        self.startNode.g_cost = 0
        self.startNode.f_cost = self.startNode.minDistance(self.endNode)
        heap = [self.startNode]
        while heap:
            node = heappop(heap)
            if node == self.endNode:  # found target
                node.traceBack()
                # Restore indications of start/end nodes
                self.startNode.color = "y"
                self.endNode.color = "r"
                return
            if node.color != VISITED:
                node.color = VISITED
                for neighbor in node.neighbors:
                    if neighbor.color != VISITED:
                        g_cost = node.g_cost + node.minDistance(neighbor)
                        if g_cost < neighbor.g_cost:
                            h_cost = neighbor.minDistance(self.endNode)
                            neighbor.g_cost = g_cost
                            neighbor.f_cost = g_cost + h_cost
                            neighbor.parent = node
                            neighbor.color = QUEUED
                            heappush(heap, neighbor)
            
            
# Example run (text-based, no pygame)
maze = Maze("""
. . . . . . . . . . . . . . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . . . . . . . . . . .
. . . . # . . . . . . . . . . . . . . . . . . . . . . . .
. . . . # . . . . . . . . y . . . . . . . . . . . . . . .
. . . . # . . . . . . . . . . . . . . . . . . # . . . . .
. . . . # . . . . . . . . . . . . . . . . . . # . . . . .
. . . . # . . . . . . . . . . . . . . . . . . # . . . . .
. . . . # . . . . . . . . . . . . . . . . . . # . . . . .
. . . . # . . . . . . . . . . . . . . . . . . # . . . . .
. . . . # . . . . . . . . . . . . . . . . . . # . . . . .
. . . . # . . . . . . . . . . . . . . . . . . # . . . . .
. . . . # . . . . . . . . . . . . . . . . . . # . . . . .
. . . . # # # # # # # # # # # # # # # # # # # # . . . . .
. . . . # . . . . . . . . . . . . . . . . . . # . . . . .
. . . . # . . . . . . . . . . . . . . . . . . # . . . . .
. . . . # . . . . . . . . . . . . . . . . . . # . . . . .
. . . . # . . . . . . . . . . . . . . . . . . # . . . . .
. . . . # . . . . . . . . . . . . . . . . . . # . . . . .
. . . . # . . . . . . . . . . . . . . . . . . # . . . . .
. . . . # . . . . . . . . . . . . . . . . . . # . . . . .
. . . . # . . . . . . . . . . . . . . . . . . # . . . . .
. . . . # . . . . . . . . . . . . . . . . . . # . . . . .
. . . . # . . . . . . . . . . . . . . . . . . # . . . . .
. . . . . . . . . . . r . . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . . . . . . . . . . .
""")

maze.aStarSearch()
print(maze)

This outputs:

. . ? ? , , , , , , , , , , , , , , , , , , ? ? . . . . .
. ? ? , , , , , , , , , , , , , , , , , , , , ? ? . . . .
? ? , , , , , , , , , , , , , , , , , , , , , , ? ? . . .
? , , , B B , , , , , , , , , , , , , , , , , , , ? ? . .
? , , B # , B , , , , , , , , , , , , , , , , , , , ? ? .
, , , B # , , B B B B B B y , , , , , , , , , , , , , ? .
, , , B # , , , , , , , , , , , , , , , , , , # , , , ? ?
, , , B # , , , , , , , , , , , , , , , , , , # , , , , ?
, , , B # , , , , , , , , , , , , , , , , , , # , , , , ?
, , , B # , , , , , , , , , , , , , , , , , , # , , , , ?
, , , B # , , , , , , , , , , , , , , , , , , # , , , , ?
, , , B # , , , , , , , , , , , , , , , , , , # , , , , ?
, , , B # , , , , , , , , , , , , , , , , , , # , , , ? ?
, , , B # , , , , , , , , , , , , , , , , , , # , , , ? .
, , , B # # # # # # # # # # # # # # # # # # # # , , ? ? .
, , , B # . . . . . . . . . . . . . . . . . . # , , ? . .
, , , B # . . . . . . . . . . . . . . . . . . # , , ? . .
? , , B # . . . . . . . . . . . . . . . . . . # , ? ? . .
? , , B # . . . . . . . . . . . . . . . . . . # , ? . . .
? , , B # . . . . . . . . . . . . . . . . . . # ? ? . . .
? ? , B # . . . . . . . . . . . . . . . . . . # . . . . .
. ? , B # . . . . . . . . . . . . . . . . . . # . . . . .
. ? ? B # . . . . . . . . . . . . . . . . . . # . . . . .
. . ? B # . . . . . . . . . . . . . . . . . . # . . . . .
. . ? B # ? ? ? ? ? ? ? . . . . . . . . . . . # . . . . .
. . ? ? B B B B B B B r . . . . . . . . . . . . . . . . .
. . . ? ? ? ? ? ? ? ? ? . . . . . . . . . . . . . . . . .
Answered By: trincot
Categories: questions Tags: , ,
Answers are sorted by their score. The answer accepted by the question owner as the best is marked with
at the top-right corner.