（1-6-03） Dijkstra算法：基于Pygame模拟的自动驾驶系统（3）

1.6.5  主程序

在本项目中有三个程序文件包含了main()函数，都可以被视为主程序，分别负责执行程序的不同逻辑。每个文件都有不同的主要功能，但它们都在程序的不同方面扮演着核心的角色。

（1）文件autopilot.py实现了一个自动驾驶汽车游戏的主程序。首先，根据选择的难度级别生成游戏世界和路线地图。然后，利用采样路径规划方法生成路径，并尝试解决规划问题。接着，根据规划结果模拟路径，并通过动作规划生成自动驾驶汽车的动作序列。最后，在自动驾驶模式下运行游戏，根据生成的动作序列控制汽车行驶，并实时更新游戏界面。

def main():
    difficulty="Easy" # 简单，中等，困难，极难，随机
    # 启动游戏
    print("Starting game")
    game=car_game(difficulty) # 实例化游戏
    print("Generating world")
    world=World(game) # 生成世界

    # 生成运动规划
    print("Generating roadmap for solver")
    Map = statespace.RoadMap(game, world)
    print("Attempting to solve")
    planner = motionplanner.SamplingPlanner(Map)
    t0 = time()
    solved, plan, exploredNodes, _ = planner.RRT()
    t1 = time() 
    if solved: 
        print(f'Path found in {t1-t0} s\n')
    else:
        print('Path not found')
    planner.simulation(plan, exploredNodes)
    actions = motionplanner.actionPlanner_SDC(Map, plan)
    # 在自动驾驶模式下运行游戏
    runGame(game, world, actions)
    pygame.quit()

def runGame(game, world, actions):
    busy = False
    ready_for_next_action = True
    angle = 0
    turn_increment = 15
    count, start_count =0, 0
    while game.run:
        game.clock.tick(100)
        count+=1
        # 更新绿色汽车
        for active_car in game.active_list:
            active_car.spritex+=active_car.vel
            active_car.updateCarOrigin()
        # 获取新事件
        for event in pygame.event.get():
            if event.type == pygame.QUIT:
                game.run = False

        keys = pygame.key.get_pressed()
        # t = pygame.time.get_ticks()
        if ready_for_next_action:
            if actions:
                action,position = actions.pop(0)
                ready_for_next_action = False
                if action == 'L':
                    angle = turn_increment
                    lane_change_count = 33
                elif action == 'R':
                    angle = -turn_increment
                    lane_change_count = 33

        if game.orange_car.x + 3*game.orange_car.car_width_px < position:
            game.orange_car.turnCar(0,game)
            start_count = count
        else:
            if count-start_count<=lane_change_count:
                game.orange_car.turnCar(angle,game)  
            elif game.orange_car.theta*(angle/abs(angle)) > 0:
                game.orange_car.turnCar(-angle,game)
            else:
                game.orange_car.theta = 0
                ready_for_next_action = True

        world.updateWinPos(game)
        world.window.redrawGameWindow(game,world.WorldSize_px)          
        if keys[pygame.K_q]:
            pygame.quit()
            game.run=False

if __name__ == '__main__':
    main()

（2）文件photo.py的功能是创建一个游戏世界并生成游戏窗口，然后在游戏窗口中绘制汽车游戏场景，并保存为图像文件。具体步骤包括导入所需的模块和函数、设置游戏难度、生成游戏世界、设置窗口大小和位置、将橙色汽车放置在指定位置、绘制游戏窗口场景、保存窗口截图为图像文件，并最终退出pygame游戏。

# 导入自定义函数
sys.path.append('Game/')
sys.path.append('MotionPlanning/')
from car import car
from window import Window
from world import World
from game import car_game
import statespace
import motionplanner
difficulty = "Easy" # 简单、中等、困难、极端、随机
print("Starting game")  # 开始游戏
game=car_game(difficulty) # 实例化游戏
print("Generating world") # 生成世界
world=World(game) # 生成世界

# 将窗口宽度和高度设置为与世界匹配
world.window.width_px=world.WorldSize_px[0]
world.window.height_px=world.WorldSize_px[1]
world.window.win = pygame.display.set_mode((world.window.width_px, world.window.height_px))

# 将窗口放置在左上角
world.window.x=0
world.window.y=0

# 将橙色汽车放在正确的位置
game.orange_car.spritex=410-game.orange_car.car_width_px/2
game.orange_car.spritey=375

# 绘制窗口并保存
world.window.redrawGameWindow(game,world.WorldSize_px)
path='world_files/'+game.gameMode+'.png'
pygame.image.save(world.window.win,path)
print("Image saved to '"+path+"'")

pygame.quit()

（3）文件worldbuilder.py实现了一个游戏世界构建器，允许用户以“建立模式”在游戏世界中放置蓝色汽车。用户可以使用箭头键移动窗口，并在所选位置放置蓝色汽车。当用户点击鼠标时，将在鼠标位置放置蓝色汽车，并保存汽车位置供以后使用。用户可以按下Q键保存并退出。

difficulty = "Easy" # 简单、中等、困难、极难、随机
road_length = 170 # 米，乘以 30 转换为像素

needCheck=True
print("这是一个用于选择蓝色汽车位置的建造模式")
while needCheck:
    check=input("此模式将覆盖以前保存的难度为 '"+difficulty+"' 的蓝色汽车位置。您确定要继续吗？请输入 'Yes' 或 'No'：")
    if check.lower()=="no" or check.lower()=='n':
        print("退出")
        needCheck=False
        exit()
    elif check.lower()=="yes" or check.lower()=='y':
        needCheck=False
        print("使用箭头键移动窗口，然后选择您想要蓝色汽车停放的位置")
        print("单击的位置会出现一个蓝色汽车，并且它的位置将被保存以供以后使用")
        print("完成后，按 Q 键保存并退出。下次您使用此难度模式运行 game.py 时，所有汽车都将停放在您放置它们的位置。")
    else:
        print("我不明白。请重试。")
bluecarlist=[]

car_pos_path = 'world_files/car_positions_'+difficulty+'.data'
length_path = 'world_files/road_length_'+difficulty+'.data'

if os.path.exists(car_pos_path):
    os.remove(car_pos_path)
if os.path.exists(length_path):
    os.remove(length_path)

print("开始游戏")
game=car_game(difficulty) # 实例化游戏

print("生成世界")
world=World(game) # 生成世界

world.window.width_px=1800
world.window.height_px=world.WorldSize_px[1]
world.window.win = pygame.display.set_mode((world.window.width_px, world.window.height_px))

# 运行游戏
while game.run:
    game.clock.tick(100)
    # 获取新事件
    for event in pygame.event.get():
        if event.type == pygame.QUIT:
            game.run = False
    keys = pygame.key.get_pressed()
    speed=10
    # 获取用于放置蓝色汽车的光标位置
    if keys[pygame.K_LEFT]:
        world.window.x-=speed
    elif keys[pygame.K_RIGHT]:
        world.window.x+=speed
    if keys[pygame.K_UP]:
        world.window.y-=speed
    elif keys[pygame.K_DOWN]:
        world.window.y+=speed
    cursor=pygame.mouse.get_pos()
    click=pygame.mouse.get_pressed()
    if click[0]==1:
        window_pos=(world.window.x,world.window.y)
        cursor_pos=(cursor[0]+window_pos[0],cursor[1]+window_pos[1])
        if world.window.top_shoulder_pos_y+world.window.y<=cursor_pos[1]<=world.window.lane_pos_y[0]+world.window.y:
            new_y=(world.window.top_shoulder_pos_y+world.window.lane_pos_y[0])/2+world.window.y

        if world.window.lane_pos_y[0]+world.window.y<=cursor_pos[1]<=world.window.lane_pos_y[1]+world.window.y:
            new_y=(world.window.lane_pos_y[0]+world.window.y+world.window.lane_pos_y[1]+world.window.y)/2
        
        if world.window.lane_pos_y[1]+world.window.y<=cursor_pos[1]<=world.window.lane_pos_y[2]+world.window.y:
            new_y=(world.window.lane_pos_y[1]+world.window.y+world.window.lane_pos_y[2]+world.window.y)/2

        if world.window.lane_pos_y[2]+world.window.y<=cursor_pos[1]<=world.window.bot_shoulder_pos_y+world.window.y:
            new_y=(world.window.lane_pos_y[2]+world.window.y+world.window.bot_shoulder_pos_y+world.window.y)/2
        new_obst=car(cursor_pos[0],new_y,"obstacle",game)
        game.obst_list.add(new_obst)
        game.all_sprites.add(new_obst)
        bluecarlist.append(cursor_pos)

    # world.updateWinPos(game)
    world.window.redrawGameWindow(game,world.WorldSize_px)
        
    if keys[pygame.K_q]:
        pygame.quit()
        game.run=False

pygame.quit()
print("保存数据")
# 从长点击中清除重复项
bluecarlist=list(dict.fromkeys(bluecarlist))
# print(bluecarlist)
with open(car_pos_path,'wb') as filehandle:
    pickle.dump(bluecarlist,filehandle)
    print("数据已保存")
with open(length_path,'wb') as filehandle:
    pickle.dump(road_length,filehandle)

执行本项目后，在Pygame中的模拟效果如图1-8所示。

图1-8  在Pygame中的模拟效果

绘制的自动驾驶路径的可视化图如图1-8所示。

图1-8  自动驾驶路径的可视化图