编写一个路径规划A星算法代码、

好的，以下是一个简单的 Python 实现：

import heapq
import math

# 定义一个节点类
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 heuristic(node, goal):
    dx = abs(node.x - goal.x)
    dy = abs(node.y - goal.y)
    return dx + dy

# A*算法主函数
def astar(start, goal, grid):
    # 初始化起点和终点
    start_node = Node(start[0], start[1])
    goal_node = Node(goal[0], goal[1])
    # 定义开放列表和关闭列表
    open_list = []
    closed_list = []
    # 将起点加入开放列表
    heapq.heappush(open_list, start_node)
    # 循环搜索直到找到终点或无法到达终点
    while open_list:
        # 从开放列表中选择f值最小的节点
        current_node = heapq.heappop(open_list)
        # 如果当前节点是终点，返回路径
        if current_node.x == goal_node.x and current_node.y == goal_node.y:
            path = []
            while current_node:
                path.append((current_node.x, current_node.y))
                current_node = current_node.parent
            return path[::-1]
        # 将当前节点加入关闭列表
        closed_list.append(current_node)
        # 搜索当前节点周围的节点
        for dx, dy in [(0, 1), (0, -1), (1, 0), (-1, 0)]:
            x = current_node.x + dx
            y = current_node.y + dy
            # 如果节点不在网格范围内或者是障碍物，则跳过
            if x < 0 or x >= len(grid) or y < 0 or y >= len(grid[0]) or grid[x][y] == 1:
                continue
            # 创建新的节点，并计算代价
            new_node = Node(x, y)
            new_node.g = current_node.g + 1
            new_node.h = heuristic(new_node, goal_node)
            new_node.f = new_node.g + new_node.h
            new_node.parent = current_node
            # 如果新节点已经在关闭列表中，则跳过
            if new_node in closed_list:
                continue
            # 如果新节点已经在开放列表中，则更新代价
            for node in open_list:
                if node.x == new_node.x and node.y == new_node.y and node.f > new_node.f:
                    open_list.remove(node)
                    heapq.heapify(open_list)
                    break
            # 将新节点加入开放列表
            heapq.heappush(open_list, new_node)
    # 如果无法到达终点，返回空路径
    return []

# 测试
grid = [
    [0, 0, 0, 0, 0],
    [0, 1, 0, 1, 0],
    [0, 0, 0, 0, 0],
    [0, 1, 0, 1, 0],
    [0, 0, 0, 0, 0]
]
start = (0, 0)
goal = (4, 4)
path = astar(start, goal, grid)
print(path)