pythonalgorithmpath-finding
A* algorithm in python having trouble with making the final path
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:
Worked better here:
Solution
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 . . . . . . . . . . . . . . . . .
. . . ? ? ? ? ? ? ? ? ? . . . . . . . . . . . . . . . . .
- matplotlib 3D scatter plot alpha varies when viewing different angles
- How to write very long string that conforms with PEP8 and prevent E501
- Getting Home Directory with pathlib
- how to avoid bot detection on websites using selenium python
- Python mock to create a fake object return a dictionary when any of its attributes are used
- Polars vs. Pandas: size and speed difference
- How to mock.patch a class imported in another module
- Python - error cannot determine truth value of Relational (Newton-Raphson)
- ProcessPoolExecutor logging fails to log inside function on Windows but not on Unix / Mac
- SQLAlchemy ORM Insert or Update when importing from JSON
- django managers vs proxy models
- Pytroch clamp for complex values
- For every identifier select only rows with largest order column
- truth value for Expr is ambiguous in with_columns ternary expansion on dates
- Remove equal characters from two python strings
- Python pyad module can't set UPN
- Macro VS Micro VS Weighted VS Samples F1 Score
- Printing a Tree data structure in Python
- How to fix/reset decreasing timestamps while preserving gaps in time-series data for CNN training?
- Test that module is NOT imported
- Pyserial module isn't installed on PATH
- Print a multiplication table in Python
- Python: ModuleNotFoundError: No module named 'xyz'
- Receiving Import Error: No Module named ***, but has __init__.py
- PyQt5 QProgressBar border radius issue
- URL-encoding and -decoding a string in Python
- Fastest way to find the smallest possible sum of the absolute differences of pairs within a single array?
- Flask: Update Code Reference for: current_app._get_current_object()
- Export Charts from Excel as images using Python
- Align yaxis label spanning two axes with yaxis labels of one axes in subplots