用python写个a星寻路

好的，下面是一个简单的 Python 实现 A* 寻路算法的示例代码：

import heapq

# 定义节点类
class Node:
    def __init__(self, x, y):
        self.x = x
        self.y = y
        self.g = 0
        self.h = 0
        self.f = 0
        self.parent = None

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

    def __eq__(self, other):
        return self.x == other.x and self.y == other.y

# 定义 A* 寻路算法函数
def astar(start, end, grid):
    # 初始化起点和终点的节点
    start_node = Node(start[0], start[1])
    end_node = Node(end[0], end[1])

    # 初始化开放列表和关闭列表
    open_list = []
    closed_list = []

    # 将起点加入开放列表
    heapq.heappush(open_list, start_node)

    # 开始寻路
    while len(open_list) > 0:
        # 获取当前节点
        current_node = heapq.heappop(open_list)

        # 如果当前节点是终点，则返回路径
        if current_node == end_node:
            path = []
            while current_node is not None:
                path.append((current_node.x, current_node.y))
                current_node = current_node.parent
            return path[::-1]

        # 将当前节点加入关闭列表
        closed_list.append(current_node)

        # 获取当前节点相邻的节点
        for neighbor in get_neighbors(current_node, grid):
            # 如果相邻节点已经在关闭列表中，则跳过
            if neighbor in closed_list:
                continue

            # 计算相邻节点的 g 值
            new_g = current_node.g + 1

            # 如果相邻节点不在开放列表中，则加入
            if neighbor not in open_list:
                neighbor.g = new_g
                neighbor.h = get_distance(neighbor, end_node)
                neighbor.f = neighbor.g + neighbor.h
                neighbor.parent = current_node
                heapq.heappush(open_list, neighbor)
            # 如果相邻节点已经在开放列表中，并且新路径更短，则更新 g 值和父节点
            elif new_g < neighbor.g:
                neighbor.g = new_g
                neighbor.f = neighbor.g + neighbor.h
                neighbor.parent = current_node

    # 如果开放列表为空，且没有找到终点，则返回空路径
    return []

# 获取当前节点相邻的节点
def get_neighbors(node, grid):
    neighbors = []
    for dx, dy in [(0, -1), (0, 1), (-1, 0), (1, 0)]:
        x = node.x + dx
        y = node.y + dy
        if x >= 0 and x < len(grid) and y >= 0 and y < len(grid[0]) and grid[x][y] == 0:
            neighbors.append(Node(x, y))
    return neighbors

# 计算两个节点之间的曼哈顿距离
def get_distance(node1, node2):
    return abs(node1.x - node2.x) + abs(node1.y - node2.y)

# 测试代码
grid = [
    [0, 0, 0, 0, 0],
    [0, 1, 1, 1, 0],
    [0, 0, 0, 0, 0],
    [0, 1, 1, 1, 0],
    [0, 0, 0, 0, 0]
]

start = (0, 0)
end = (4, 4)

path = astar(start, end, grid)
print(path)

在上面的代码中，我们首先定义了一个 `Node` 类来表示节点，其中包含了节点的坐标、节点的 `g` 值、节点的 `h` 值、节点的 `f` 值和节点的父节点。

然后我们定义了一个 `astar` 函数来实现 A* 寻路算法。该函数接受起点、终点和二维网格数组作为输入，返回一条从起点到终点的路径。

在 `astar` 函数中，我们首先初始化起点和终点的节点，并将起点加入开放列表。然后开始进行寻路，直到开放列表为空或者找到终点为止。在每次循环中，我们从开放列表中取出 f 值最小的节点作为当前节点，将其加入关闭列表，并获取其相邻的节点。对于每个相邻节点，如果其已经在关闭列表中，则跳过；否则，我们计算其新的 `g` 值，并根据需要更新其 `f` 值、父节点，并将其加入开放列表。如果相邻节点已经在开放列表中，并且新路径更短，则更新其 `g` 值和父节点。最后，如果找到终点，则返回路径；否则，返回空路径。

在 `get_neighbors` 函数中，我们根据当前节点的坐标和二维网格数组获取当前节点相邻的节点。在 `get_distance` 函数中，我们计算两个节点之间的曼哈顿距离。

最后，我们对代码进行了简单的测试，使用一个 5x5 的网格数组来测试 A* 寻路算法是否正确。