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 . . . . . . . . . . . . . . . . .
. . . ? ? ? ? ? ? ? ? ? . . . . . . . . . . . . . . . . .
- VSCode Python extension loading forever, saying “Reactivating terminals”
- How to disable functools @lru_cache when developing locally
- Plotly: How to apply different titles for each different subplots?
- Does my code prevent directory traversal?
- sklearn can't find lapack in new conda environment
- pyinstaller error: OSError: [WinError 6] The handle is invalid
- Difficulty understanding nested loops with addition, range and print function
- Save json request to a file
- Hierarchical Bayes for R or Python
- What is the aim of cv2.Rodrigues() applying on rvec in context of camera calibration
- How to add scrollbar to Python Folium popup?
- Type hinting to return the same type of Sized iterable as the input?
- Differences between columns containing lists
- Escaping quotes in Python subprocesses for Windows
- How do I copy a string to the clipboard?
- Python code to retrieve system info in format
- Multidimensional matrix permutation Julia vs. Python disagreement
- How to use threading.Lock in async function while object can be accessed from multiple thread
- How to save data into two related tables in Django?
- How does defining what a function will return work
- How to improve pandas DF processing time on different combinations of calculated data
- How split (chunk) a list of customers along month/year?
- Sagemaker batch transformer with my own pre-trained model
- Given an array of N integers, return the max sum of 3 NON-adjacent elements
- Error "table ... already exits" when using to_sql(...,if_exists='append') with Pandas and SQLite
- Saving tkinter canvas with black background and white line objects using PIL results in fully white image
- change grid interval in astropy curved axis
- Qt/PySide6: What is the best way to implement an infinite data fetching loop with a QThread?
- Number of installations statistics for PyPI packages?
- how can I do a maximum likelihood regression using scipy.optimize.minimize