graduation-project/navigate.py

import heapq


# 可行走的位置（即值为0，或目标位置）
def is_walkable(pos):
    r, c = pos
    if 0 <= r < len(parking_lot) and 0 <= c < len(parking_lot[0]):
        return parking_lot[r][c] == 0 or pos == goal
    return False


# 曼哈顿距离作为启发函数
def heuristic(a, b):
    return abs(a[0] - b[0]) + abs(a[1] - b[1])


# A*搜索
def a_star(start, goal):
    open_set = []
    heapq.heappush(open_set, (0 + heuristic(start, goal), 0, start))
    came_from = {}  # 路径追踪
    g_score = {start: 0}

    while open_set:
        _, current_cost, current = heapq.heappop(open_set)

        if current == goal:
            # 还原路径
            path = [current]
            while current in came_from:
                current = came_from[current]
                path.append(current)
            return path[::-1]  # 反转

        # 邻接节点（上下左右）
        for dr, dc in [(-1, 0), (1, 0), (0, -1), (0, 1)]:
            neighbor = (current[0] + dr, current[1] + dc)

            if not is_walkable(neighbor):
                continue

            tentative_g = current_cost + 1
            if neighbor not in g_score or tentative_g < g_score[neighbor]:
                g_score[neighbor] = tentative_g
                f = tentative_g + heuristic(neighbor, goal)
                heapq.heappush(open_set, (f, tentative_g, neighbor))
                came_from[neighbor] = current

    return None  # 找不到路径


if __name__ == "__main__":
    # 停车场状态矩阵
    parking_lot = [
        [1, 1, 1, 0, 3, 2, 1],
        [1, 1, 1, 0, 3, 1, 1],
        [0, 0, 0, 0, 0, 0, 0],
        [0, 0, 0, 0, 0, 0, 0],
        [0, 1, 1, 1, 1, 1, 0],
        [0, 2, 2, 2, 2, 2, 0],
        [0, 0, 0, 0, 0, 0, 0],
    ]

    # 起点和终点（行, 列）
    start = (0, 3)
    goal = (3, 5)
    # 执行 A* 路径搜索
    path = a_star(start, goal)

    # 输出结果
    if path:
        print("找到路径:")
        for step in path:
            print(step)
    else:
        print("找不到路径。")