onsite_parking/planner/run_test_b03.py

import os
import sys
sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
                "/../../onsite_parking/")
import json
import input.make_map as mp
from planner.hybridastar import planner as planner
from input import make_car
from utils import replay
from utils import map_display as mdp
import matplotlib.pyplot as plt
import numpy as np
def main():

    # 输入input文件夹下场景文件
    map_path = '../input/B03.json'
    mdp.map_display(map_path) #  仅绘制地图

    ox, oy,sp,gp = mp.make_map(map_path)
    sx, sy, syaw0 = sp['x'], sp['y'], sp['yaw']
    C = make_car.C

    # 获取目标停车位
    park = '7'
    gx, gy, gyaw0 = gp[park]['x_end'], gp[park]['y_end'], gp[park]['yaw']
    print(gx,gy)
    if park == '62'or park == '61':
        for i in range(sx - 300, sx + 2700, 1):
            ox.append(i)
            oy.append(5600)
        for i in range(sx + 700, sx + 4200, 1):
            ox.append(i)
            oy.append(6500)
        for i in range(6500, 7700, 1):
            ox.append(1000)
            oy.append(i)
        x = np.linspace(3000, 4500, 500)
        y = np.linspace(5600, 5000, 500)
        for i in range(len(x)):
            ox.append(x[i])
            oy.append(y[i])
        x = np.linspace(6000, 7800, 500)
        y = np.linspace(5000, 5300, 500)
        for i in range(len(x)):
            ox.append(x[i])
            oy.append(y[i])
        x = np.linspace(8200, 10500, 500)
        y = np.linspace(5720, 7700, 500)
        for i in range(len(x)):
            ox.append(x[i])
            oy.append(y[i])
        x = np.linspace(6000, 9200, 500)
        y = np.linspace(4050, 4400, 500)
        for i in range(len(x)):
            ox.append(x[i])
            oy.append(y[i])
    if park == '60' or park == '59' or park == '58' or park == '57'or park == '56'or park == '55':
        for i in range(sx - 300, sx + 2700, 1):
            ox.append(i)
            oy.append(5600)
        for i in range(sx + 700, sx + 4200, 1):
            ox.append(i)
            oy.append(6500)
        for i in range(6500, 7700, 1):
            ox.append(1000)
            oy.append(i)
        x = np.linspace(3000, 4500, 500)
        y = np.linspace(5600, 5000, 500)
        for i in range(len(x)):
            ox.append(x[i])
            oy.append(y[i])
        x = np.linspace(6000, 9200, 500)
        y = np.linspace(5000, 4400, 500)
        for i in range(len(x)):
            ox.append(x[i])
            oy.append(y[i])
        x = np.linspace(6000, 7000, 500)
        y = np.linspace(4000, 3100, 500)
        for i in range(len(x)):
            ox.append(x[i])
            oy.append(y[i])
    if park == '54':
        for i in range(sx - 300, sx + 2700, 1):
            ox.append(i)
            oy.append(5600)
        for i in range(sx + 700, sx + 4200, 1):
            ox.append(i)
            oy.append(6500)
        for i in range(6500, 7700, 1):
            ox.append(1000)
            oy.append(i)
        x = np.linspace(3000, 4500, 500)
        y = np.linspace(5600, 5000, 500)
        for i in range(len(x)):
            ox.append(x[i])
            oy.append(y[i])
        x = np.linspace(6000, 9550, 500)
        y = np.linspace(5000, 4500, 500)
        for i in range(len(x)):
            ox.append(x[i])
            oy.append(y[i])
        x = np.linspace(6000, 7000, 500)
        y = np.linspace(4000, 3100, 500)
        for i in range(len(x)):
            ox.append(x[i])
            oy.append(y[i])
    if park == '53' or park == '52':
        for i in range(sx - 300, sx + 2700, 1):
            ox.append(i)
            oy.append(5600)
        for i in range(sx + 700, sx + 4200, 1):
            ox.append(i)
            oy.append(6500)
        for i in range(6500, 7700, 1):
            ox.append(1000)
            oy.append(i)
        x = np.linspace(3000, 4500, 500)
        y = np.linspace(5600, 5000, 500)
        for i in range(len(x)):
            ox.append(x[i])
            oy.append(y[i])
        x = np.linspace(6000, 9900, 500)
        y = np.linspace(5000, 5200, 500)
        for i in range(len(x)):
            ox.append(x[i])
            oy.append(y[i])

    if park == '51' or park == '50' or park == '49':
        for i in range(sx - 300, sx + 2700, 1):
            ox.append(i)
            oy.append(5600)
        for i in range(sx + 700, sx + 4200, 1):
            ox.append(i)
            oy.append(6500)
        for i in range(6500, 7700, 1):
            ox.append(1000)
            oy.append(i)
        x = np.linspace(3000, 4500, 500)
        y = np.linspace(5600, 5000, 500)
        for i in range(len(x)):
            ox.append(x[i])
            oy.append(y[i])
        x = np.linspace(6000, 9100, 500)
        y = np.linspace(5000, 5000, 500)
        for i in range(len(x)):
            ox.append(x[i])
            oy.append(y[i])
        # x = np.linspace(6000, 10500, 500)
        # y = np.linspace(700, 700, 500)
        # for i in range(len(x)):
        #     ox.append(x[i])
        #     oy.append(y[i])
        x = np.linspace(7000, 7000, 500)
        y = np.linspace(3100, 5000, 500)
        for i in range(len(x)):
            ox.append(x[i])
            oy.append(y[i])
        x = np.linspace(7000, 8000, 500)
        y = np.linspace(3100, 3100, 500)
        for i in range(len(x)):
            ox.append(x[i])
            oy.append(y[i])
    if park == '48':
        for i in range(sx - 300, sx + 2700, 1):
            ox.append(i)
            oy.append(5600)
        for i in range(sx + 700, sx + 4200, 1):
            ox.append(i)
            oy.append(6500)
        for i in range(6500, 7700, 1):
            ox.append(1000)
            oy.append(i)
        x = np.linspace(3000, 4500, 500)
        y = np.linspace(5600, 5000, 500)
        for i in range(len(x)):
            ox.append(x[i])
            oy.append(y[i])
        x = np.linspace(6000, 9100, 500)
        y = np.linspace(5000, 4400, 500)
        for i in range(len(x)):
            ox.append(x[i])
            oy.append(y[i])
        # x = np.linspace(6000, 10500, 500)
        # y = np.linspace(700, 700, 500)
        # for i in range(len(x)):
        #     ox.append(x[i])
        #     oy.append(y[i])
        # x = np.linspace(7000, 7000, 500)
        # y = np.linspace(3100, 5000, 500)
        # for i in range(len(x)):
        #     ox.append(x[i])
        #     oy.append(y[i])
        x = np.linspace(7000, 10500, 500)
        y = np.linspace(3100, 3100, 500)
        for i in range(len(x)):
            ox.append(x[i])
            oy.append(y[i])
    if park == '47':
        for i in range(sx - 300, sx + 2700, 1):
            ox.append(i)
            oy.append(5600)
        for i in range(sx + 700, sx + 4200, 1):
            ox.append(i)
            oy.append(6500)
        for i in range(6500, 7700, 1):
            ox.append(1000)
            oy.append(i)
        x = np.linspace(3000, 4500, 500)
        y = np.linspace(5600, 5000, 500)
        for i in range(len(x)):
            ox.append(x[i])
            oy.append(y[i])
        x = np.linspace(6000, 9100, 500)
        y = np.linspace(5000, 3100, 500)
        for i in range(len(x)):
            ox.append(x[i])
            oy.append(y[i])
        # x = np.linspace(6000, 10500, 500)
        # y = np.linspace(700, 700, 500)
        # for i in range(len(x)):
        #     ox.append(x[i])
        #     oy.append(y[i])
        x = np.linspace(7000, 7000, 500)
        y = np.linspace(3100, 5000, 500)
        for i in range(len(x)):
            ox.append(x[i])
            oy.append(y[i])
        x = np.linspace(7000, 10500, 500)
        y = np.linspace(3100, 3100, 500)
        for i in range(len(x)):
            ox.append(x[i])
            oy.append(y[i])
    if park == '46' or park == '45':
        for i in range(sx - 300, sx + 1900, 1):
            ox.append(i)
            oy.append(5600)
        for i in range(sx + 700, sx + 4200, 1):
            ox.append(i)
            oy.append(6500)
        for i in range(6500, 7700, 1):
            ox.append(1000)
            oy.append(i)
        x = np.linspace(2100, 4500, 500)
        y = np.linspace(5600, 5000, 500)
        for i in range(len(x)):
            ox.append(x[i])
            oy.append(y[i])
        x = np.linspace(6000, 9100, 500)
        y = np.linspace(5000, 5000, 500)
        for i in range(len(x)):
            ox.append(x[i])
            oy.append(y[i])
    if park == '44' or park == '43'or park == '42'or park == '41'or park == '40':
        for i in range(sx - 300, sx + 1900, 1):
            ox.append(i)
            oy.append(5600)
        for i in range(sx + 700, sx + 4200, 1):
            ox.append(i)
            oy.append(6500)
        for i in range(6500, 7700, 1):
            ox.append(1000)
            oy.append(i)
        x = np.linspace(2100, 4500, 500)
        y = np.linspace(5600, 5000, 500)
        for i in range(len(x)):
            ox.append(x[i])
            oy.append(y[i])
        x = np.linspace(7000, 10500, 500)
        y = np.linspace(3100, 3100, 500)
        for i in range(len(x)):
            ox.append(x[i])
            oy.append(y[i])
        x = np.linspace(6000, 10500, 500)
        y = np.linspace(5000, 5000, 500)
        for i in range(len(x)):
            ox.append(x[i])
            oy.append(y[i])
        x = np.linspace(8500, 8500, 500)
        y = np.linspace(3100, 6000, 500)
        for i in range(len(x)):
            ox.append(x[i])
            oy.append(y[i])
    if park == '39':
        for i in range(sx - 300, sx + 1900, 1):
            ox.append(i)
            oy.append(5600)
        for i in range(sx + 700, sx + 4200, 1):
            ox.append(i)
            oy.append(6500)
        for i in range(6500, 7700, 1):
            ox.append(1000)
            oy.append(i)
        x = np.linspace(2100, 4500, 500)
        y = np.linspace(5600, 5000, 500)
        for i in range(len(x)):
            ox.append(x[i])
            oy.append(y[i])
        x = np.linspace(7000, 10500, 500)
        y = np.linspace(3100, 3100, 500)
        for i in range(len(x)):
            ox.append(x[i])
            oy.append(y[i])
        x = np.linspace(6000, 8000, 500)
        y = np.linspace(5700, 5700, 500)
        for i in range(len(x)):
            ox.append(x[i])
            oy.append(y[i])
        x = np.linspace(7500, 7500, 500)
        y = np.linspace(3100, 6000, 500)
        for i in range(len(x)):
            ox.append(x[i])
            oy.append(y[i])
        x = np.linspace(7000, 8500, 500)
        y = np.linspace(1700, 1700, 500)
        for i in range(len(x)):
            ox.append(x[i])
            oy.append(y[i])
    if park == '38':
        for i in range(sx - 300, sx + 1900, 1):
            ox.append(i)
            oy.append(5600)
        for i in range(sx + 700, sx + 4200, 1):
            ox.append(i)
            oy.append(6500)
        for i in range(6500, 7700, 1):
            ox.append(1000)
            oy.append(i)
        x = np.linspace(2100, 4500, 500)
        y = np.linspace(5600, 5000, 500)
        for i in range(len(x)):
            ox.append(x[i])
            oy.append(y[i])
        x = np.linspace(7000, 10500, 500)
        y = np.linspace(3100, 3100, 500)
        for i in range(len(x)):
            ox.append(x[i])
            oy.append(y[i])
        x = np.linspace(6000, 8000, 500)
        y = np.linspace(5700, 5700, 500)
        for i in range(len(x)):
            ox.append(x[i])
            oy.append(y[i])
        x = np.linspace(8500, 8500, 500)
        y = np.linspace(3100, 6000, 500)
        for i in range(len(x)):
            ox.append(x[i])
            oy.append(y[i])
    if park == '37'or park == '36' or park == '35' or park == '34':
        for i in range(sx - 300, sx + 1900, 1):
            ox.append(i)
            oy.append(5600)
        for i in range(sx + 700, sx + 4200, 1):
            ox.append(i)
            oy.append(6500)
        for i in range(6500, 7700, 1):
            ox.append(1000)
            oy.append(i)
        x = np.linspace(2100, 4500, 500)
        y = np.linspace(5600, 5000, 500)
        for i in range(len(x)):
            ox.append(x[i])
            oy.append(y[i])
        x = np.linspace(6000, 9200, 500)
        y = np.linspace(4000, 4400, 500)
        for i in range(len(x)):
            ox.append(x[i])
            oy.append(y[i])
    if park == '33':
        for i in range(sx - 300, sx + 1900, 1):
            ox.append(i)
            oy.append(5600)
        for i in range(sx + 700, sx + 4200, 1):
            ox.append(i)
            oy.append(6500)
        for i in range(6500, 7700, 1):
            ox.append(1000)
            oy.append(i)
        x = np.linspace(2100, 4500, 500)
        y = np.linspace(5600, 5000, 500)
        for i in range(len(x)):
            ox.append(x[i])
            oy.append(y[i])
        x = np.linspace(6000, 9900, 500)
        y = np.linspace(4000, 5200, 500)
        for i in range(len(x)):
            ox.append(x[i])
            oy.append(y[i])
        x = np.linspace(9000, 9000, 500)
        y = np.linspace(6700, 4000, 500)
        for i in range(len(x)):
            ox.append(x[i])
            oy.append(y[i])

    if park == '23' or park == '22' or park == '21':
        x = np.linspace(4500, 4500, 500)
        y = np.linspace(6000, 5000, 500)
        for i in range(len(x)):
            ox.append(x[i])
            oy.append(y[i])
        x = np.linspace(0, 2100, 500)
        y = np.linspace(5600, 5600, 500)
        for i in range(len(x)):
            ox.append(x[i])
            oy.append(y[i])
        x = np.linspace(0, 1000, 500)
        y = np.linspace(3000, 3000, 500)
        for i in range(len(x)):
            ox.append(x[i])
            oy.append(y[i])
        x = np.linspace(3500, 3500, 500)
        y = np.linspace(3100, 7000, 500)
        for i in range(len(x)):
            ox.append(x[i])
            oy.append(y[i])
        x = np.linspace(0, 2100, 500)
        y = np.linspace(4000, 4000, 500)
        for i in range(len(x)):
            ox.append(x[i])
            oy.append(y[i])
        x = np.linspace(2100, 2100, 100)
        y = np.linspace(4600, 5100, 100)
        for i in range(len(x)):
            ox.append(x[i])
            oy.append(y[i])
        x = np.linspace(1000, 4000, 500)
        y = np.linspace(6600, 6600, 500)
        for i in range(len(x)):
            ox.append(x[i])
            oy.append(y[i])
        x = np.linspace(1000, 1000, 100)
        y = np.linspace(7700, 6600, 100)
        for i in range(len(x)):
            ox.append(x[i])
            oy.append(y[i])
        x = np.linspace(3000, 3500, 100)
        y = np.linspace(3100, 3100, 100)
        for i in range(len(x)):
            ox.append(x[i])
            oy.append(y[i])
    if park == '20' or park == '19' or park == '18' or park == '17':
        x = np.linspace(4500, 4500, 500)
        y = np.linspace(6000, 5000, 500)
        for i in range(len(x)):
            ox.append(x[i])
            oy.append(y[i])
    if park == '15' or park == '14' or park == '13' or park == '12' or park == '11' or park == '10':
        x = np.linspace(4500, 4500, 500)
        y = np.linspace(6000, 5000, 500)
        for i in range(len(x)):
            ox.append(x[i])
            oy.append(y[i])
        x = np.linspace(3500, 3500, 500)
        y = np.linspace(3100, 7000, 500)
        for i in range(len(x)):
            ox.append(x[i])
            oy.append(y[i])
        x = np.linspace(0, 2100, 500)
        y = np.linspace(5600, 5600, 500)
        for i in range(len(x)):
            ox.append(x[i])
            oy.append(y[i])
        if park == '10':
            x = np.linspace(0, 2100, 500)
            y = np.linspace(4100, 4100, 500)
            for i in range(len(x)):
                ox.append(x[i])
                oy.append(y[i])
        x = np.linspace(2100, 2100, 500)
        y = np.linspace(4500, 5700, 500)
        for i in range(len(x)):
            ox.append(x[i])
            oy.append(y[i])
        x = np.linspace(1000, 4000, 500)
        y = np.linspace(6600, 6600, 500)
        for i in range(len(x)):
            ox.append(x[i])
            oy.append(y[i])
        x = np.linspace(1000, 1000, 100)
        y = np.linspace(7700, 6600, 100)
        for i in range(len(x)):
            ox.append(x[i])
            oy.append(y[i])
        x = np.linspace(1000, 3500, 100)
        y = np.linspace(3100, 3100, 100)
        for i in range(len(x)):
            ox.append(x[i])
            oy.append(y[i])
    if park == '9' or park == '8' or park == '7' or park == '6':

        x = np.linspace(4500, 4500, 500)
        y = np.linspace(6000, 5000, 500)
        for i in range(len(x)):
            ox.append(x[i])
            oy.append(y[i])
        x = np.linspace(3500, 3500, 500)
        y = np.linspace(3100, 7000, 500)
        for i in range(len(x)):
            ox.append(x[i])
            oy.append(y[i])
        x = np.linspace(0, 2100, 500)
        y = np.linspace(5600, 5600, 500)
        for i in range(len(x)):
            ox.append(x[i])
            oy.append(y[i])

        x = np.linspace(2100, 2100, 500)
        y = np.linspace(4500, 5700, 500)
        for i in range(len(x)):
            ox.append(x[i])
            oy.append(y[i])
        x = np.linspace(1000, 4000, 500)
        y = np.linspace(6600, 6600, 500)
        for i in range(len(x)):
            ox.append(x[i])
            oy.append(y[i])
        x = np.linspace(1000, 1000, 100)
        y = np.linspace(7700, 6600, 100)
        for i in range(len(x)):
            ox.append(x[i])
            oy.append(y[i])
        x = np.linspace(1000, 3500, 100)
        y = np.linspace(3100, 3100, 100)
        for i in range(len(x)):
            ox.append(x[i])
            oy.append(y[i])
    if park == '5' or park == '4' or park == '3' or park == '2' or park == '1':
        pass

    plt.plot(ox, oy, ",k")
    plt.show()
    # 规划算法
    path = planner.hybrid_astar_planning(sx, sy, syaw0, gx, gy, gyaw0, ox, oy, C.XY_RESO, C.YAW_RESO)
    
    # 算法测试结果保存
    if not path:
        print("Searching failed!")
        return
    output_dit={
        "output_x":path.x,
        "output_y": path.y,
        "output_yaw": path.yaw,
        "output_dir": path.direction,
    }
    with open(f"../output/result_{map_path.split('/')[-1].split('.json')[0]}_{park}.json", "w") as file:
        json.dump(output_dit, file)

    # 仿真回放
    result_path = f"../output/result_{map_path.split('/')[-1].split('.json')[0]}_{park}.json"
    replay.replay(map_path, result_path)
if __name__ == '__main__':
    main()