Python中如何实现A*算法？

import heapq

class Node:
    def __init__(self, position, g=0, h=0):
        self.position = position
        self.g = g
        self.h = h
        self.f = g + h

    def __lt__(self, other):
        return self.f < other.f

def heuristic(a, b):
    return abs(a[0] - b[0]) + abs(a[1] - b[1])

def a_star(start, goal, grid):
    start_node = Node(start, 0, heuristic(start, goal))
    goal_node = Node(goal, 0, 0)

    open_list = []
    heapq.heappush(open_list, start_node)
    closed_set = set()

    came_from = {}

    while open_list:
        current = heapq.heappop(open_list)

        if current.position == goal_node.position:
            path = []
            while current.position in came_from:
                path.append(current.position)
                current = came_from[current.position]
            path.append(start)
            return path[::-1]

        closed_set.add(current.position)

        for neighbor_pos in get_neighbors(current.position, grid):
            if neighbor_pos in closed_set:
                continue

            tentative_g = current.g + 1

            neighbor = Node(neighbor_pos, tentative_g, heuristic(neighbor_pos, goal))

            if neighbor not in open_list:
                heapq.heappush(open_list, neighbor)
                came_from[neighbor.position] = current
            elif tentative_g < neighbor.g:
                neighbor.g = tentative_g
                neighbor.f = neighbor.g + neighbor.h
                came_from[neighbor.position] = current
                heapq.heapify(open_list)

    return None

def get_neighbors(position, grid):
    neighbors = []
    for dx, dy in [(0, 1), (1, 0), (0, -1), (-1, 0)]:
        new_x, new_y = position[0] + dx, position[1] + dy
        if 0 <= new_x < len(grid) and 0 <= new_y < len(grid[0]) and grid[new_x][new_y] == 0:
            neighbors.append((new_x, new_y))
    return neighbors

# 示例使用
grid = [
    [0, 0, 0, 0, 1],
    [1, 1, 0, 0, 0],
    [0, 0, 0, 1, 0],
    [0, 1, 1, 0, 0],
    [0, 0, 0, 0, 0]
]
start = (0, 0)
goal = (4, 4)

path = a_star(start, goal, grid)
print(path)