用A*写一段路径规划代码

好的，以下是一个简单的路径规划代码，使用 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  # 评估函数 f = g + h
        self.parent = None  # 父节点

    def __lt__(self, other):
        return self.f < other.f  # 用于 heapq 模块的比较函数

# 定义地图类
class Map:
    def __init__(self, width, height, obstacles):
        self.width = width
        self.height = height
        self.obstacles = obstacles

    # 判断节点是否合法
    def is_valid_node(self, node):
        return 0 <= node.x < self.width and 0 <= node.y < self.height and node not in self.obstacles

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

    # 获取某个节点的邻居节点
    def get_neighbors(self, node):
        neighbors = []
        for dx, dy in [(0, 1), (1, 0), (0, -1), (-1, 0)]:
            neighbor = Node(node.x + dx, node.y + dy)
            if self.is_valid_node(neighbor):
                neighbors.append(neighbor)
        return neighbors

# A*算法
def astar_search(map, start, end):
    # 初始化起点和终点
    start.g = 0
    start.h = map.manhattan_distance(start, end)
    start.f = start.h
    open_list = [start]
    close_list = []

    # 开始搜索
    while open_list:
        current_node = heapq.heappop(open_list)
        close_list.append(current_node)

        # 到达终点，返回路径
        if current_node == end:
            path = []
            while current_node:
                path.append(current_node)
                current_node = current_node.parent
            return path[::-1]

        # 扩展当前节点的邻居节点
        for neighbor in map.get_neighbors(current_node):
            # 如果邻居节点已经在关闭列表中，跳过
            if neighbor in close_list:
                continue

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

            # 如果邻居节点已经在开放列表中，更新 g 值和 f 值
            if neighbor in open_list:
                if new_g < neighbor.g:
                    neighbor.g = new_g
                    neighbor.f = neighbor.g + neighbor.h
                    neighbor.parent = current_node
                    heapq.heapify(open_list)
            # 如果邻居节点不在开放列表中，加入开放列表
            else:
                neighbor.g = new_g
                neighbor.h = map.manhattan_distance(neighbor, end)
                neighbor.f = neighbor.g + neighbor.h
                neighbor.parent = current_node
                heapq.heappush(open_list, neighbor)

    # 没有找到路径，返回空
    return None

在使用时，需要先创建一个地图对象，然后定义起点和终点，调用 A* 算法进行搜索，最后得到路径。示例如下：

# 创建地图
obstacles = [Node(2, 1), Node(2, 2), Node(2, 3)]
map = Map(5, 5, obstacles)

# 定义起点和终点
start = Node(0, 0)
end = Node(4, 4)

# 进行搜索
path = astar_search(map, start, end)

# 输出路径
if path:
    for node in path:
        print("({}, {})".format(node.x, node.y))
else:
    print("Path not found.")