A* algorithm only exploring a few nodes before stopping - without reaching goal node

73 Views Asked by At

I am attempting to implement the A* algorithm but it only goes to three nodes before just stopping completely.

This is the algorithm code:

def AStar(start_node, end_node):
    openSet = PriorityQueue()
    openSet.enequeue(0, start_node)

    infinity = float("inf")

    gCost = {}
    fCost = {}
    cameFrom = {}

    for node in graph:
        gCost[node] = infinity
        fCost[node] = infinity
    gCost[start_node] = 0
    fCost[start_node] = heuristic(start_node, end_node)

    while not openSet.isEmpty():
        current = openSet.dequeue()  # Doesn't work yet

        if current == end_node:
            RetracePath(cameFrom, end_node)

        for neighbour in find_neighbors(start_node, graph):
            tempGCost = gCost[current] + 1

            if tempGCost < gCost[neighbour]:
                cameFrom[neighbour] = current
                gCost[neighbour] = tempGCost
                fCost[neighbour] = tempGCost + heuristic(neighbour, end_node)

                if not openSet.contains(neighbour):
                    openSet.enequeue(fCost[neighbour], neighbour)

        print(f"Came from: {cameFrom}\nCurrent: {current}")
    return False

And this is the code that finds the adjacent nodes:


def find_neighbors(node, graph):
    x, y = node
    neighbors = []

    right_neighbor = (x + 1, y)
    left_neighbor = (x - 1, y)
    lower_neighbor = (x, y + 1)
    upper_neighbor = (x, y - 1)

    if right_neighbor in graph:
        neighbors.append(right_neighbor)
    if left_neighbor in graph:
        neighbors.append(left_neighbor)
    if lower_neighbor in graph:
        neighbors.append(lower_neighbor)
    if upper_neighbor in graph:
        neighbors.append(upper_neighbor)

And this is an example of what is being outputted:

Enemy coords: (6, 2)
Player coords: 10, 2
Enemy neighbours: [(7, 2), (6, 3)]
Priority Queue: [[0, (6, 2)]]
Priority Queue: [[4, (7, 2)]]
Priority Queue: [[4, (7, 2)], [6, (6, 3)]]
Came from: {(7, 2): (6, 2), (6, 3): (6, 2)}
Current: (6, 2)
Came from: {(7, 2): (6, 2), (6, 3): (6, 2)}
Current: (7, 2)
Came from: {(7, 2): (6, 2), (6, 3): (6, 2)}
Current: (6, 3)

Before it just stops the code without reaching the goal coordinates (player coords)

lmk if you have any questions about the code, thank you.

1

There are 1 best solutions below

0
حمزة نبيل On

Your code check only the neighbors of the start node, when you call find_neighbors you always use start_node you should use current instead :

for neighbour in find_neighbors(current, graph):
    # your code