A 路径搜索算法：正确实现邻居节点遍历的关键

def AStar(start_node, end_node):
    # ... (初始化代码) ...

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

        if current == end_node:
            RetracePath(cameFrom, end_node)
            return True # 找到路径

        # 错误：这里始终使用 start_node 寻找邻居
        for neighbour in find_neighbors(start_node, graph):
            tempGCost = gCost[current] + 1

            if tempGCost < gCost[neighbour]:
                cameFrom[neighbour] = current
                gCost[neighbour] = tempGCost
                fCost[neighbour] = tempGCost + heuristic(neighbour, end_node)

                if not openSet.contains(neighbour):
                    openSet.enequeue(fCost[neighbour], neighbour)
    return False # 未找到路径

def AStar(start_node, end_node, graph, heuristic):
    openSet = PriorityQueue()
    openSet.enqueue(0, start_node) # PriorityQueue通常需要一个优先级和一个值

    infinity = float("inf")

    gCost = {} # 存储从起点到n的实际代价
    fCost = {} # 存储从起点经过n到终点的总估计代价
    cameFrom = {} # 存储每个节点的前驱，用于路径回溯

    # 初始化所有节点的gCost和fCost为无穷大
    for node in graph: # 假设graph是一个可迭代的节点集合
        gCost[node] = infinity
        fCost[node] = infinity

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

    while not openSet.isEmpty():
        # 从开放列表中取出fCost最小的节点
        # 注意：这里的dequeue方法需要返回节点本身，而不是优先级
        priority, current = openSet.dequeue() 

        if current == end_node:
            return RetracePath(cameFrom, end_node) # 找到目标，回溯路径并返回

        # 正确：探索当前节点的邻居
        for neighbour in find_neighbors(current, graph):
            # 假设每一步的代价为1
            tempGCost = gCost[current] + 1 

            # 如果通过当前节点到达邻居的路径更优
            if tempGCost < gCost.get(neighbour, infinity): # 使用.get避免KeyError
                cameFrom[neighbour] = current
                gCost[neighbour] = tempGCost
                fCost[neighbour] = tempGCost + heuristic(neighbour, end_node)

                # 如果邻居不在开放列表中，则加入
                # 这里的openSet.contains(neighbour)需要PriorityQueue支持
                # 更好的做法是，如果已经在openSet中，则更新其优先级
                if not openSet.contains(neighbour): # 假设PriorityQueue有contains方法
                    openSet.enqueue(fCost[neighbour], neighbour)
                else:
                    # 如果邻居已在openSet中，更新其优先级（如果新的fCost更小）
                    # 实际的PriorityQueue实现可能需要一个update_priority方法
                    pass 
        # print(f"Came from: {cameFrom}\nCurrent: {current}") # 调试输出
    return False # 未找到路径

# 辅助函数：寻找邻居节点 (假设为2D网格)
def find_neighbors(node, graph):
    x, y = node
    neighbors = []

    possible_neighbors = [
        (x + 1, y), # 右
        (x - 1, y), # 左
        (x, y + 1), # 下
        (x, y - 1)  # 上
    ]

    for neighbor_coord in possible_neighbors:
        if neighbor_coord in graph: # 检查邻居是否在图中（有效且可通行）
            neighbors.append(neighbor_coord)
    return neighbors

# 辅助函数：启发式函数 (曼哈顿距离)
def heuristic(node, goal):
    return abs(node[0] - goal[0]) + abs(node[1] - goal[1])

# 辅助函数：回溯路径 (示例)
def RetracePath(cameFrom, end_node):
    path = []
    current = end_node
    while current in cameFrom:
        path.append(current)
        current = cameFrom[current]
    path.append(current) # 添加起始节点
    return path[::-1] # 反转路径以从起点到终点