My A* pathfinding algorithm does not always get the shortest path

Question

thanks in advance for any help.

I am making a pathfinder visualiser using python in pygame. I have tried to make the A* algorithm, but sometimes it does not find the shortest path. I have been looking through several previous questions with the same issue, which has led me to believe it may be a problem with the heuristic. If I set the hueristic value to 0, then the algorithm becomes dijkstra's and always gets the shortest path.

A grid is used for the algorithm, with x being the number row and y being the number column (I believe might be the other way around but it doesnt matter)

Each square on the grid is an object, with x and y values, as well as a gScore, hScore and fScore. On initialisation these are all set to None.

I also have some functions at the bottom to do calculations, such as find the lowest fScore node from an array, find the gScore, find the hScore, fScore and get the distance between two grid squares.

I think the problem is in the hueristic function and have tried several different methods of fixing to no avail. From looking at the code below would anyone be able to see the problem, or point me in the right direction? Any help is much appreciated .

For simplicity I have only included the A* function without any of the pygame stuff, but I can add the entire program if need be, including the gridsquare object.

def a_star():

    for row in grid:
        for square in row:
            if square.state == "start_pos":
                start_pos = square

            elif square.state == "end_pos":
                end_pos = square


    start_pos.gScore = find_g(start_pos, start_pos)
    start_pos.hScore = find_h(start_pos, end_pos)
    start_pos.fScore = find_f(start_pos.gScore, start_pos.hScore)


    openList = [start_pos]
    closedList = []


    while len(openList) > 0:

        current_node = get_lowest_f_node(openList)

        if current_node.state == "end_pos":
            print("found")

            path = [end_pos]
            node = current_node
            while node.parent != None:
                time.sleep(SHORTEST_PATH_DELAY)
                node = node.parent
                path.append(node)
            return

        openList.remove(current_node)
        closedList.append(current_node)


        x = current_node.x
        y = current_node.y

        # get nodes around current node
        node1 = grid[x][y - 1]
        node2 = grid[x][y + 1]
        node3 = grid[x - 1][y]
        node4 = grid[x + 1][y]

        successor_nodes = [node1, node2, node3, node4]

        for node in successor_nodes:

            # check if walkable
            if (node.state == "wall") or (node in closedList):
                continue


            if node.gScore == None:
                node.gScore = current_node.gScore

            tentative_g_score = current_node.gScore + get_distance(node, current_node)

            if (node in closedList) and (tentative_g_score >= node.gScore):
                continue

            if (node not in openList) or (tentative_g_score < node.gScore):
                node.parent = current_node
                node.gScore = tentative_g_score
                node.fScore = node.gScore + find_h(node, end_pos)
                if node not in openList:
                    openList.append(node)






def get_lowest_f_node(array):
    min_f = min(array, key = attrgetter("fScore"))
    return min_f


# distance from current node and start node
def find_g(current, start_pos):
    g = get_distance(current, start_pos)
    return g


# distance from current node and target / destination / finish node
def find_h(current, end_pos):
    h = get_distance(current, end_pos)
    return h


# hscore and gscore added together
def find_f(score1, score2):
    return score1 + score2


# distance from 2 points
def get_distance(start, end):
    x1 = start.x
    y1 = start.y
    x2 = end.x
    y2 = end.y
    distancex = sqr(x2 - x1)
    distancey = sqr(y2 - y1)
    #distance = sqrt(distancex + distancey)
    distance = distancex + distancey
    return distance

def sqr(number):
    return number * number

Below are some images of the result of the path finding, with different patters. The starting node is always the bottom red square.

^^^ This is where the A* algorithm finds the correct, shortest path. All good.

^^^This is where A* finds a path, but it is not the shortest path. This is what I am trying to fix, any help is much appreciated.

^^^ this is dijkstra's finding the correct path when presented with the same arrangement of walls.

I am very grateful for any help.

Answer 1

<==== Download This image

I'm no expert on A* but awhile I wrote a script for a YouTube video I was going to produce.

If you want it then it's here: https://pastebin.com/WycrpAfZ

You can also view it here:

import math, random, sys
import pygame
from pygame.locals import *

# exit the program
def events():
    for event in pygame.event.get():
        if event.type == QUIT or (event.type == KEYDOWN and event.key == K_ESCAPE):
            pygame.quit()
            sys.exit()

# define display surface            
W, H = 1920, 1080
HW, HH = W / 2, H / 2
AREA = W * H

# initialise display
pygame.init()
pygame.font.init()
CLOCK = pygame.time.Clock()
FONT_SMALL = pygame.font.Font(None, 26)
FONT_LARGE = pygame.font.Font(None, 50)
DS = pygame.display.set_mode((W, H))
pygame.display.set_caption("code.Pylet - Template")
FPS = 1

# define some colors
BLACK = (0, 0, 0, 255)
WHITE = (255, 255, 255, 255)
RED = (255, 0, 0, 255)
GREEN = (0, 128, 0, 255)
BLUE = (0, 0, 255, 255)
PURPLE = (255, 255, 0, 255)

# define node class
class node:
    def __init__(self, x, y, obstacle):
        self.x = x
        self.y = y
        self.pos = (x, y)
        self.h = 0
        self.g = 0
        self.f = 0
        self.obstacle = obstacle
        self.other = None
        self.parent = None

    def neighbourPos(self, offset):
        return (self.x + offset[0], self.y + offset[1])

    def draw(self, size, color = None, id = None, surface = None):
        global text, FONT_SMALL, FONT_LARGE
        if not surface: surface = pygame.display.get_surface()
        pos = (self.x * size[0], self.y * size[1])
        if not color:
            if not self.obstacle:
                if not self.other: pygame.draw.rect(surface, BLACK, pos + size, 0)
                else: pygame.draw.rect(surface, BLUE, pos + size, 0)
            else:
                pygame.draw.rect(surface, WHITE, pos + size, 0)
        else:
            pygame.draw.rect(surface, color, pos + size, 0)
        pygame.draw.rect(surface, WHITE, pos + size, 1)
        if self.f:
            text(FONT_SMALL, "G:{0}".format(self.g), pos[0] + 5, pos[1] + 5, 0, 0, surface)
            text(FONT_SMALL, "H:{0}".format(self.h), pos[0] + size[0] - 5, pos[1] + 5, 1, 0, surface)
            text(FONT_LARGE, "F:{0}".format(self.f),  pos[0] + size[0] / 2, pos[1] + size[1] / 2 , 2, 2, surface)
            if not id == None:
                text(FONT_SMALL, "{0}".format(id), pos[0] + 5, pos[1] + size[1] - 5, 0, 1, surface)


def drawNodes(n, ms, cs):
    for x in range(ms[0]):
        for y in range(ms[1]):
            n[x][y].draw(cs)

def drawNodeList(node_list, cs, color):
    id = 0
    for n in node_list:
        n.draw(cs, color, id)
        id += 1

def heuristics(pos1, pos2):
    return int(math.hypot(pos1[0] - pos2[0], pos1[1] - pos2[1]) * 10)

def text(font, string, x, y, xJustify = None, yJustify = None, surface = None):
    global WHITE
    if not surface: surface = pygame.display.get_surface()
    textSurface = font.render(string, 1, WHITE)
    textRect = textSurface.get_rect()
    if xJustify == 1:
        x -= textRect.width
    elif xJustify == 2:
        x -= textRect.center[0]
    if yJustify == 1:
        y -= textRect.height
    elif yJustify == 2:
        y -= textRect.center[1]
    surface.blit(textSurface, (x, y))

map = pygame.image.load("test.png").convert()
map_size = map_width, map_height = map.get_rect().size
cell_size = (W / map_width, H / map_height)

#create list of nodes
nodes = list([])
for x in range(map_width):
    nodes.append(list([]))
    for y in range(map_height):
        color = map.get_at((x, y))
        if color != WHITE:
            nodes[x].append(node(x, y, False))
            if color == BLUE:
                start = nodes[x][y]
                start.other = True
            elif color == RED:
                end = nodes[x][y]
                end.other = True
        else:
            nodes[x].append(node(x, y, True))


# This list contains relative x & y positions to reference a node's neighbour 
NEIGHBOURS = list([(-1, -1), (0, -1), (1, -1), (1, 0), (1, 1), (0, 1), (-1, 1), (-1, 0)])           




# the closed list contains all the nodes that have been considered economical viable.
# By that I mean a node that has been closer to the end node than any other in the open list at one time
closed = list([])

# The open list contains all the closed list's neighbours that haven't been identified as being economically sound node yet
open = list([])
open.append(start) # add the start node so that we can then add it's neighbours



# if the algorithm finds the end node then pathFound will be true otherwise it's false. 
# Once it becomes true there's no more calculations to do so the path finding script will be skipped over
pathFound = False
completedPath = list([]) # 

# main loop
while True:
    DS.fill(BLACK)  
    drawNodes(nodes, map_size, cell_size)
    drawNodeList(open, cell_size, GREEN)
    drawNodeList(closed, cell_size, RED)
    if pathFound: drawNodeList(completedPath, cell_size, PURPLE)
    pygame.display.update()

    # wait for user to press mouse button
    while not pygame.mouse.get_pressed()[0]:
        events()
    while pygame.mouse.get_pressed()[0]:
        events()

    # if we've found the quickest path from start node to end node then just draw, no need continue path finding
    if pathFound: continue
    if not open: continue


    # get lowest f from the open list, the node with the lowest f is the most economical in terms of the path towards the end node
    openNodeWithlowestF = open[0]
    for o in open:
        if  o.f < openNodeWithlowestF.f: openNodeWithlowestF = o

    mostEconomicalNodeSoFar = openNodeWithlowestF # let's make this more readable! Economical means the best path to the end given the choices but not definitive.

    # remove the mostEconomicalNodeSoFar from the open list
    open.remove(mostEconomicalNodeSoFar)
    # add mostEconomicalNodeSoFar to the closed list
    closed.append(mostEconomicalNodeSoFar)

    # if the mostEconomicalNodeSoFar is equal to the end node then we've reach our target
    if mostEconomicalNodeSoFar == end:
        temp = end
        while temp.parent:
            completedPath.append(temp)
            temp = temp.parent
        completedPath.append(start)
        pathFound = True
        # get the path etc

    # iterate through the list of neighbours belonging to the mostEconomicalNodeSoFar. Why?
    for neighbourOffset in NEIGHBOURS:
        nx, ny = mostEconomicalNodeSoFar.neighbourPos(neighbourOffset)

        if nx < 0 or nx >= map_width or ny < 0 or ny >= map_height: continue
        neighbour = nodes[nx][ny] # create a variable to represent the mostEconomicalNodeSoFar's neighbour
        if neighbour.obstacle: continue # if the mostEconomicalNodeSoFar's neighbouring node is an obstacle then we can't ...?
        if neighbour in closed: continue # if the mostEconomicalNodeSoFar's neighbouring node is in the closed list then we can't ...?

        # now we need to see if the mostEconomicalNodeSoFar's neighbour is more economical ...?
        hypotheticalFScore = mostEconomicalNodeSoFar.g + heuristics(neighbour.pos, mostEconomicalNodeSoFar.pos)
        NeighbourIsBetterThanMostEconomicalNodeSoFar = False # Yes it's a long variable name but it describes what it is so all is good!

        # is this neighbour already in open list? if it is then we don't want to be adding it again. to chec
        if not neighbour in open:
            NeighbourIsBetterThanMostEconomicalNodeSoFar = True
            neighbour.h = heuristics(neighbour.pos, end.pos)
            open.append(neighbour)
        elif hypotheticalFScore < neighbour.g:
            NeighbourIsBetterThanMostEconomicalNodeSoFar = True

        if NeighbourIsBetterThanMostEconomicalNodeSoFar:
            neighbour.parent = mostEconomicalNodeSoFar
            neighbour.g = hypotheticalFScore
            neighbour.f = neighbour.g + neighbour.h
    #sys.exit()