A* path finding algo sometimes doesn't find path even when there is one (Python)
I made a custom path A* Pathfinding Algorithm in Python, but it sometimes doesn't even find path to the end node even when there clearly is a path. Here is my implementation.
# this is my Node class. I am representing the whole maze as a matrix and every cell
# of that matrix is a Node Object
class Node():
def __init__(self, i, j):
self.i = i
self.j = j
self.isWall = False
self.isOpen = None
self.f = 0
self.g = 0
self.h = 0
self.neighbors = []
self.previous = None
def __repr__(self):
return f'< i = {self.i}, j = {self.j}, previous = {self.previous} >'
def add_neighbors(self, grid, diagonal):
i = self.i
j = self.j
if i > 0:
self.neighbors.append(grid[i - 1][j])
if i < len(grid) - 1:
self.neighbors.append(grid[i + 1][j])
if j > 0:
self.neighbors.append(grid[i][j - 1])
if j < len(grid) - 1:
self.neighbors.append(grid[i][j + 1])
if diagonal:
# for diagonal neighbors
# down and right
if i < len(grid) - 1 and j < len(grid) - 1:
self.neighbors.append(grid[i + 1][j + 1])
# up and right
if i > 0 and j < len(grid) - 1:
self.neighbors.append(grid[i - 1][j + 1])
#down and left
if i < len(grid) - 1 and j > 0:
self.neighbors.append(grid[i + 1][j - 1])
#up and left
if i > 0 and j > 0:
self.neighbors.append(grid[i - 1][j - 1])
iterate through and set up the nodes
def make_grid(length):
main_grid = []
for i in range(length):
lst = []
for j in range(length):
node = Node(i, j)
# 30 % chance that the current node will be set as a wall
if random.randrange(1, 101) > 70 and i != 0 and j != 0: node.isWall = True
lst.append(node)
main_grid.append(lst)
for i in range(length):
for j in range(length):
main_grid[i][j].add_neighbors(main_grid, diagonal = True)
return main_grid
# Below is how the above function 'make_grid' is called
# making the grid
grid_len = 25
main_grid = make_grid(grid_len)
path = [] # to reconstruct the optimal path
Below is the HScore function I'm using
# HScore function
def getHScore(node, endNode):
return sqrt(abs(node.i - endNode.i)**2 + abs(node.j - endNode.j)**2)
Below is the actual Algorithm implementation
# A* PATHFINDING ALGORITHM
def aStar(start_node, end_node):
# node.f = node.g + node.h
# node.g = distance of current node from the starting node
# node.h = distance of current node from the end node
start_node.g = 0
start_node.h = getHScore(start_node, end_node)
start_node.f = start_node.g + start_node.h
open_set = [start_node]
closed_set = []
if start_node.isWall:
print("The start node is a wall")
return
while True:
if len(open_set) < 1:
print('No Solutions Found')
break
current_node = open_set[0]
for node in open_set:
if node.f < current_node.f:
current_node = node
current_node.isOpen = True
# print(f'current_node = {current_node.i, current_node.j}', end = " ")
if current_node == end_node:
temp = end_node
path.append(temp)
while temp.previous is not None:
path.append(temp.previous)
temp = temp.previous
print("DONE")
colorFinalPath(main_grid)
break
# current_node.isPath = True
current_node.isOpen = False
open_set.remove(current_node)
closed_set.append(current_node)
for neighbor in current_node.neighbors:
# assuming 1 as the distance btw two neighbouring points that aren't diagonally
# neighbors
# need to add 1.14 if neighbor is diagonal. add propery to node class to check if neighbor is diagonal
if neighbor in closed_set:
continue
tempG = current_node.g + getHScore(current_node, neighbor)
if neighbor not in open_set and not neighbor.isWall:
neighbor.g = tempG
open_set.append(neighbor)
neighbor.isOpen = True
neighbor.previous = current_node
if tempG >= neighbor.g:
continue # there is no better path
# neighbor was found in the open set, so we check if we can get to it in
# a better way as tempG is now less than neighbor.g
neighbor.previous = current_node
neighbor.g = tempG
neighbor.h = getHScore(neighbor, end_node)
neighbor.f = neighbor.g + neighbor.h
show_steps(main_grid, start_node, end_node)
Some Screen shots In the third picture there is clearly a path between the starting Node (top left) and the end Node (bottom right) but it doesn't find any solutions.
Please tell me what's wrong with my implementation. Any help is appreciated
I see some issues in this piece of code:
tempG = current_node.g + getHScore(current_node, neighbor)
if neighbor not in open_set and not neighbor.isWall:
neighbor.g = tempG
open_set.append(neighbor)
neighbor.isOpen = True
neighbor.previous = current_node
if tempG >= neighbor.g:
continue # there is no better path
When the neighbor is a wall, you should skip it immediately. So add at the top:
if neighbor.isWall: continueThis also means you can remove the wall check from the
ifyou already hadThe condition to check there is no better path will also be true when you just had set the
gcomponent for the first time, i.e. when the middle part was executed. This should not happen. So change thatifto anelif:if neighbor not in open_set: # ... etc ... elif tempG >= neighbor.g: continue # there is no better pathYour
make_gridcode could mark the end node as a wall. You do not reject that situation, and then your code willcontinueand skip that as a neighbor to put in the open set. It is not clear from your image whether this happened, as you colored the ending node in blue.
Then, less of a problem, but you will sometimes call getHScore multiple times for the same node. Obviously, that function will return the same value for each of those calls. So you could improve on that. For example, by moving that line in an if condition:
if neighbor.h == 0:
neighbor.h = getHScore(neighbor, end_node)
I don't know whether it was intended but a diagonal step gets a cost of 2 (1²+1²), and it has no advantage over a 2-step walk to that same square. This is a tiny detail as you would visit those nodes first via the diagonal step and then ignore paths with the same cost.