A*経路探索を、障害物グリッドで一手ずつ見る

from heapq import heappop, heappush

def astar(grid, start, goal):
    """4-neighbour A* for a grid where 1 means blocked."""
    height, width = len(grid), len(grid[0])
    frontier = [(0, start)]
    came_from = {start: None}
    cost = {start: 0}

    def heuristic(cell):
        return abs(cell[0] - goal[0]) + abs(cell[1] - goal[1])

    while frontier:
        _, current = heappop(frontier)
        if current == goal:
            break

        row, col = current
        for next_cell in ((row-1, col), (row+1, col),
                          (row, col-1), (row, col+1)):
            r, c = next_cell
            if not (0 <= r < height and 0 <= c < width):
                continue
            if grid[r][c] == 1:
                continue

            new_cost = cost[current] + 1
            if next_cell not in cost or new_cost < cost[next_cell]:
                cost[next_cell] = new_cost
                priority = new_cost + heuristic(next_cell)
                heappush(frontier, (priority, next_cell))
                came_from[next_cell] = current

    if goal not in came_from:
        return []

    path, current = [], goal
    while current is not None:
        path.append(current)
        current = came_from[current]
    return path[::-1]