I’m implementing an A* algorithm for a navigation system, but I’ve encountered a problem when handling temporary obstacles. The current setup navigates through the shortest path and attempts diagonal movements when necessary. However, it fails to consider alternative routes when the direct path is blocked by a temporary obstacle.

Given a grid where:

A is the starting point,
B, C are waypoints that we have to reach.
M is a temporary obstacle,
D is a valid diagonal move, and
N is a walkable alternative route
0 is non walkable path
1 is walkable path

I want the algorithm to consider alternative routes when the shortest path is blocked. Here’s a visual representation of my grid:

[0,0,0,0,0,0,0,0,M,1,1,1,0,0,0]
[0,0,0,0,0,0,0,1,D,0,0,1,0,0,0]
[0,0,0,0,0,0,1,1,0,0,0,1,0,0,0]
[0,0,0,0,0,1,0,0,0,1,1,1,0,0,0]
[0,0,0,0,0,0,0,A,M,N,1,0,0,0,0]
[0,0,0,0,0,0,0,N,N,N,1,0,0,0,0]
[0,0,0,0,0,0,0,0,0,0,0,0,0,0,0]
[0,0,0,0,0,0,0,0,0,0,0,0,0,0,0]
[0,0,0,0,0,0,0,0,0,0,0,0,0,0,0]
[0,0,0,0,0,0,0,0,0,0,0,0,0,0,0]

import heapq
from typing import List, Tuple
import numpy as np
import pyautogui

def is_walkable(pos: Tuple[int, int, int], binary_matrices: List[np.ndarray]) -> bool:
    x, y, z = pos
    if 0 <= z < len(binary_matrices) and 0 <= y < binary_matrices[z].shape[0] and 0 <= x < binary_matrices[z].shape[1]:
        return binary_matrices[z][y, x] == 1
    return False

def get_neighbors(current: Tuple[int, int, int], binary_matrices: List[np.ndarray], use_diagonals: bool = False) -> List[Tuple[int, int, int]]:
    x, y, z = current
    neighbors = []
    directions = [(1, 0), (-1, 0), (0, 1), (0, -1)]  # Cardinal directions: right, left, down, up

    if use_diagonals:
        directions += [(1, 1), (1, -1), (-1, 1), (-1, -1)]  # Diagonal directions

    for dx, dy in directions:
        nx, ny = x + dx, y + dy
        if is_walkable((nx, ny, z), binary_matrices):
            neighbors.append((nx, ny, z))
    return neighbors

def find_path(start: Tuple[int, int, int], end: Tuple[int, int, int], binary_matrices: List[np.ndarray], search_range: int = 5, use_diagonals: bool = False) -> List[Tuple[int, int, int]]:
    def heuristic(a: Tuple[int, int, int], b: Tuple[int, int, int]) -> float:
        return abs(a[0] - b[0]) + abs(a[1] - b[1])  # Manhattan distance

    start_pixel = get_pixel_from_coordinate(start)
    end_pixel = get_pixel_from_coordinate(end)
    z = start[2]

    start_pixel = (*start_pixel, z)
    end_pixel = (*end_pixel, z)

    open_set = []
    heapq.heappush(open_set, (0, start_pixel))
    came_from = {}
    g_score = {start_pixel: 0}
    f_score = {start_pixel: heuristic(start_pixel, end_pixel)}

    print(f"Start pixel: {start_pixel}, End pixel: {end_pixel}")

    attempt_counter = 0
    max_attempts = 30
    used_diagonal = False

    while open_set:
        current = heapq.heappop(open_set)[1]
        print(f"Exploring: {current}")

        if current == end_pixel:
            path_found = True
            break

        neighbors = get_neighbors(current, binary_matrices, use_diagonals)
        for neighbor in neighbors:
            tentative_g_score = g_score[current] + 1
            if neighbor not in g_score or tentative_g_score < g_score[neighbor]:
                came_from[neighbor] = current
                g_score[neighbor] = tentative_g_score
                f_score[neighbor] = tentative_g_score + heuristic(neighbor, end_pixel)
                if neighbor not in [i[1] for i in open_set]:
                    heapq.heappush(open_set, (f_score[neighbor], neighbor))
                    print(f"  Added neighbor: {neighbor}")

        if len(open_set) == 0:
            attempt_counter += 1
            print(attempt_counter)
            if attempt_counter >= max_attempts and not use_diagonals:
                print(f"Attempted {attempt_counter} times without reaching the goal. Switching to diagonal movements.")
                use_diagonals = True
                used_diagonal = True
                attempt_counter = 0
                open_set = []
                heapq.heappush(open_set, (0, start_pixel))
                came_from = {}
                g_score = {start_pixel: 0}
                f_score = {start_pixel: heuristic(start_pixel, end_pixel)}
            else:
                print(f"No progress made after {attempt_counter} attempts. Retrying with the current settings.")
        else:
            attempt_counter = 0
            
    if not path_found:
        print("No path found")
        return []

    path = []
    while current in came_from:
        path.append(get_coordinate_from_pixel(current[:2], current[2]))
        current = came_from[current]
    path.append(start)
    path = list(reversed(path))

    if used_diagonal:
        filtered_path = [path[0]]
        for i in range(1, len(path)):
            dx = path[i][0] - filtered_path[-1][0]
            dy = path[i][1] - filtered_path[-1][1]
            if dx != 0 and dy != 0:
                continue
            filtered_path.append(path[i])
        path = filtered_path
    
    return path

def move_character(current_pos: Tuple[int, int, int], next_pos: Tuple[int, int, int], use_diagonal: bool = False):
    dx = next_pos[0] - current_pos[0]
    dy = next_pos[1] - current_pos[1]

    if use_diagonal:
        if dx > 0 and dy > 0:
            pyautogui.press('num3')
        elif dx > 0 and dy < 0:
            pyautogui.press('num9')
        elif dx < 0 and dy > 0:
            pyautogui.press('num1')
        elif dx < 0 and dy < 0:
            pyautogui.press('num7')
    else:
        if dx > 0:
            pyautogui.press('right')
        elif dx < 0:
            pyautogui.press('left')

        if dy > 0:
            pyautogui.press('down')
        elif dy < 0:
            pyautogui.press('up')

pathfinding.py

def navigate_to_waypoint(target_waypoint: Tuple[int, Tuple[int, int, int]], binary_matrices: List[np.ndarray], timeout: int = CONFIG['timeout']) -> bool:
start_time = time.time()
waypoint_id, waypoint_coords = target_waypoint
max_attempts = CONFIG['max_attempts']
attempt_counter = 0

while True:
    if time.time() - start_time > timeout:
        print(f"Timeout reached for waypoint {waypoint_id}. Moving to next waypoint.")
        return False
    try:
        current_pos = get_absolute_coordinates()
        print(f"Current position (game coordinates): {current_pos}")
        current_pixel = get_pixel_from_coordinate(current_pos)
        print(f"Current position (pixel coordinates): {current_pixel}")

        if current_pos[0] == waypoint_coords[0] and current_pos[1] == waypoint_coords[1] and current_pos[2] == waypoint_coords[2]:
            print(f"Reached waypoint {waypoint_id}: {waypoint_coords}")
            return True
        
        path = find_path(current_pos, waypoint_coords, binary_matrices)
        
        if not path:
            print("No valid path found. Retrying...")
            time.sleep(1)
            continue
        
        next_pos = path[1] if len(path) > 1 else path[0]
        print(f"Moving to next position: {next_pos}")
        move_character(current_pos, next_pos)

        if len(path) > 2 and path[1] != next_pos:
            attempt_counter = 0
        else:
            attempt_counter += 1

        if attempt_counter >= max_attempts:
            print(f"Attempted {attempt_counter} times without reaching the goal.")
            attempt_counter = 0

        time.sleep(0.1)
    except Exception as e:
        print(f"An error occurred: {e}")

I tried different “heuristic, neighbors exploration”.
I want a navigation system that can be able to take a walkable alternative route to reach the waypoint if there are temporary obstacles.