A 算法路径探索中断问题解析与修正

# 原始问题代码片段
# ...
while not openSet.isEmpty():
    current = openSet.dequeue()

    if current == end_node:
        RetracePath(cameFrom, end_node)

    # 错误之处：总是探索 start_node 的邻居
    for neighbour in find_neighbors(start_node, graph): 
        tempGCost = gCost[current] + 1
        # ... 后续逻辑

# 修正后的代码片段
# ...
while not openSet.isEmpty():
    current = openSet.dequeue()

    if current == end_node:
        return RetracePath(cameFrom, end_node) # 修正：到达目标后应返回路径

    # 正确做法：探索当前节点 (current) 的邻居
    for neighbour in find_neighbors(current, graph_nodes): 
        tempGCost = gCost[current] + 1
        # ... 后续逻辑

import heapq

# 辅助类：优先队列
class PriorityQueue:
    def __init__(self):
        self.elements = [] # 存储 (priority, item) 元组
        self.item_map = {} # 用于快速检查元素是否存在及更新优先级 {item: current_priority}

    def isEmpty(self):
        return len(self.elements) == 0

    def enqueue(self, priority, item):
        # 如果元素已存在且新优先级更高（代价更低），则更新
        # 这里我们只在新的优先级更优时才更新，或者元素不存在时添加
        if item not in self.item_map or priority < self.item_map[item]:
            heapq.heappush(self.elements, (priority, item))
            self.item_map[item] = priority # 记录或更新元素的当前最佳优先级

    def dequeue(self):
        while self.elements:
            priority, item = heapq.heappop(self.elements)
            # 检查 item_map，确保我们处理的是最新的、最低优先级的元素
            # 如果 item_map 中的优先级更高，说明这个元素是旧的、无效的（已被更优路径更新）
            if item in self.item_map and priority == self.item_map[item]:
                del self.item_map[item] # 从 map 中移除，表示已处理
                return item
        return None # 队列为空或所有元素都已无效

    def contains(self, item):
        return item in self.item_map

# 启发式函数（曼哈顿距离，适用于四向移动的网格图）
def heuristic(a, b):
    (x1, y1) = a
    (x2, y2) = b
    return abs(x1 - x2) + abs(y1 - y2)

# 路径回溯函数
def RetracePath(cameFrom, end_node):
    path = []
    current = end_node
    while current in cameFrom:
        path.append(current)
        current = cameFrom[current]
    path.append(current) # 添加起始节点
    path.reverse()
    return path

# 查找邻居函数（适用于网格图，假设 graph_nodes 是一个包含所有可通行坐标的集合）
def find_neighbors(node, graph_nodes):
    x, y = node
    neighbors = []
    possible_neighbors = [
        (x + 1, y), (x - 1, y), (x, y + 1), (x, y - 1)
    ]
    for neighbor in possible_neighbors:
        if neighbor in graph_nodes: # 检查邻居是否在图中且可通行
            neighbors.append(neighbor)
    return neighbors

# 修正后的 A* 算法主函数
def AStar_corrected(start_node, end_node, graph_nodes):
    openSet = PriorityQueue()
    openSet.enqueue(0, start_node) # 初始节点的 fCost 为 0 + h(start, end)

    infinity = float("inf")

    gCost = {}
    fCost = {}
    cameFrom = {}

    # 初始化所有节点的 gCost 和 fCost 为无穷大
    for node in graph_nodes:
        gCost[node] = infinity
        fCost[node] = infinity

    gCost[start_node] = 0
    fCost[start_node] = heuristic(start_node, end_node)

    while not openSet.isEmpty():
        current = openSet.dequeue()

        # 如果当前节点是目标节点，则回溯路径并返回
        if current == end_node:
            return RetracePath(cameFrom, end_node)

        # 遍历当前节点的所有邻居
        for neighbour in find_neighbors(current, graph_nodes):
            # 假设每一步的移动代价为 1
            tempGCost = gCost[current] + 1 

            # 如果通过当前节点到达