python二维路径规划dijstra算法

下面是一个简单的二维路径规划Dijkstra算法的Python实现：

import heapq

# 定义二维空间中的节点类
class Node:
    def __init__(self, x, y):
        self.x = x
        self.y = y
        self.cost = float('inf')  # 节点的实际代价
        self.parent = None

    # 定义节点之间的比较方法
    def __lt__(self, other):
        return self.cost < other.cost

# 定义二维空间中的网格类
class Grid:
    def __init__(self, width, height):
        self.width = width
        self.height = height
        self.grid = [[' ' for y in range(height)] for x in range(width)]

    # 定义网格中的障碍物和自由空间
    def set_obstacle(self, x, y):
        self.grid[x][y] = '#'

    def set_free(self, x, y):
        self.grid[x][y] = ' '

    def is_free(self, x, y):
        return self.grid[x][y] == ' '

    # 定义二维空间中的路径规划方法
    def find_path(self, start, goal):
        # 初始化起始节点和目标节点
        start.cost = 0
        open_list = [start]
        closed_list = []

        # 开始搜索路径
        while open_list:
            current = heapq.heappop(open_list)  # 弹出开放列表中最小代价的节点
            if current == goal:
                path = []
                while current:
                    path.append(current)
                    current = current.parent
                return path[::-1]  # 返回反转的路径

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

            # 遍历当前节点的所有相邻节点
            for dx in range(-1, 2):
                for dy in range(-1, 2):
                    if dx == dy == 0:
                        continue
                    x, y = current.x + dx, current.y + dy
                    if x < 0 or x >= self.width or y < 0 or y >= self.height:
                        continue
                    if self.grid[x][y] == '#':
                        continue
                    neighbor = Node(x, y)
                    neighbor_cost = current.cost + 1  # 假设所有相邻节点的代价为1
                    if neighbor_cost < neighbor.cost:
                        neighbor.cost = neighbor_cost
                        neighbor.parent = current

                        # 如果相邻节点已经在闭合列表中，则跳过
                        if neighbor in closed_list:
                            continue

                        # 如果相邻节点已经在开放列表中，则更新其代价和父节点
                        if neighbor in open_list:
                            index = open_list.index(neighbor)
                            open_list[index].cost = neighbor_cost
                            open_list[index].parent = current
                            heapq.heapify(open_list)
                        else:  # 否则，将相邻节点加入开放列表
                            heapq.heappush(open_list, neighbor)

        # 如果没有找到路径，则返回空列表
        return []

# 测试路径规划算法
if __name__ == '__main__':
    grid = Grid(5, 5)
    grid.set_obstacle(1, 1)
    grid.set_obstacle(1, 2)
    grid.set_obstacle(2, 1)
    grid.set_obstacle(2, 2)

    start = Node(0, 0)
    goal = Node(4, 4)

    path = grid.find_path(start, goal)
    if path:
        for node in path:
            print(node.x, node.y)
    else:
        print('No path found')

上面的代码实现了一个简单的二维空间中的路径规划Dijkstra算法，其中障碍物用字符“#”表示，自由空间用空格表示。你可以根据需要修改障碍物和自由空间的表示方式，以及相邻节点的代价计算方法。