A*算法路标地图python代码

以下是使用A*算法在路标地图上进行路径搜索的Python代码示例：

import heapq

class Node:
    def __init__(self, name, position):
        self.name = name
        self.position = position
        self.neighbors = {}
        self.gCost = float('inf')  # 初始代价设为无穷大
        self.hCost = 0  # 启发式代价设为0
        self.fCost = float('inf')  # 总代价设为无穷大
        self.parent = None

    def add_neighbor(self, neighbor, distance):
        self.neighbors[neighbor] = distance

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

def distance(current, neighbor):
    # 计算两个节点之间的距离（如欧几里得距离、曼哈顿距离等）
    return ((current.position[0] - neighbor.position[0]) ** 2 + (current.position[1] - neighbor.position[1]) ** 2) ** 0.5

def heuristic(node, goalNode):
    # 计算启发式代价（如曼哈顿距离、欧几里得距离等）
    return distance(node, goalNode)

def A_star(start, goal):
    # 初始化节点和边
    nodes = {
        'A': Node('A', (0, 0)),
        'B': Node('B', (1, 2)),
        'C': Node('C', (3, 1)),
        'D': Node('D', (5, 3)),
        'E': Node('E', (2, 4)),
        'F': Node('F', (4, 5)),
        'G': Node('G', (6, 6))
    }

    nodes['A'].add_neighbor(nodes['B'], 2)
    nodes['A'].add_neighbor(nodes['C'], 4)
    nodes['B'].add_neighbor(nodes['C'], 1)
    nodes['B'].add_neighbor(nodes['D'], 7)
    nodes['C'].add_neighbor(nodes['E'], 3)
    nodes['D'].add_neighbor(nodes['F'], 5)
    nodes['E'].add_neighbor(nodes['F'], 2)
    nodes['F'].add_neighbor(nodes['G'], 3)

    # 初始化起始节点和终点节点
    startNode = nodes[start]
    goalNode = nodes[goal]

    # 初始化开放列表和关闭列表
    openList = []
    heapq.heapify(openList)
    heapq.heappush(openList, startNode)
    closedList = set()

    while openList:
        # 从开放列表中选择具有最低总代价的节点
        currentNode = heapq.heappop(openList)

        # 如果当前节点是目标节点，则找到了最短路径
        if currentNode == goalNode:
            return reconstructPath(currentNode)

        # 将当前节点添加到关闭列表中
        closedList.add(currentNode)

        # 遍历当前节点的邻居
        for neighbor, distance in currentNode.neighbors.items():
            # 如果邻居节点已在关闭列表中，则跳过
            if neighbor in closedList:
                continue

            # 计算邻居节点的代价
            neighbor.gCost = currentNode.gCost + distance
            neighbor.hCost = heuristic(neighbor, goalNode)
            neighbor.fCost = neighbor.gCost + neighbor.hCost

            # 如果邻居节点不在开放列表中，则添加到开放列表
            if neighbor not in openList:
                heapq.heappush(openList, neighbor)
                neighbor.parent = currentNode
            # 否则，