onsite_parking/planner/run_test_b01.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/B01.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 = '26'
    gx, gy, gyaw0 = gp[park]['x_end'], gp[park]['y_end'], gp[park]['yaw']
    print(gx,gy)

    if park == '3' or park == '2'or park == '1'or park == '6'or park == '8':
        x = np.linspace(10300, 10300, 500)
        y = np.linspace(7000, 1000, 500)
        for i in range(len(x)):
            ox.append(x[i])
            oy.append(y[i])
        x = np.linspace(8800, 8800, 500)
        y = np.linspace(2800, 5500, 500)
        for i in range(len(x)):
            ox.append(x[i])
            oy.append(y[i])
        x = np.linspace(7200, 8800, 500)
        y = np.linspace(1300, 2800, 500)
        for i in range(len(x)):
            ox.append(x[i])
            oy.append(y[i])
        x = np.linspace(9200, 10300, 500)
        y = np.linspace(870, 1800, 500)
        for i in range(len(x)):
            ox.append(x[i])
            oy.append(y[i])
    if park == '5'or park == '7' :
        x = np.linspace(10300, 10300, 500)
        y = np.linspace(7000, 1000, 500)
        for i in range(len(x)):
            ox.append(x[i])
            oy.append(y[i])
        x = np.linspace(8800, 8800, 500)
        y = np.linspace(2800, 5500, 500)
        for i in range(len(x)):
            ox.append(x[i])
            oy.append(y[i])
        x = np.linspace(7200, 8800, 500)
        y = np.linspace(1300, 2800, 500)
        for i in range(len(x)):
            ox.append(x[i])
            oy.append(y[i])
        x = np.linspace(8800, 10300, 500)
        y = np.linspace(870, 1800, 500)
        for i in range(len(x)):
            ox.append(x[i])
            oy.append(y[i])
    if park == '4': pass
    if park == '9' or  park == '10'or  park == '11'or  park == '12'or  park == '13'or  park == '14'or  park == '15':
        x = np.linspace(5800, 1500, 500)
        y = np.linspace(2000, 2000, 500)
        for i in range(len(x)):
            ox.append(x[i])
            oy.append(y[i])
        x = np.linspace(8300, 8300, 500)
        y = np.linspace(2800, 500, 500)
        for i in range(len(x)):
            ox.append(x[i])
            oy.append(y[i])

        x = np.linspace(4000, 1400, 500)
        y = np.linspace(2900, 2900, 500)
        for i in range(len(x)):
            ox.append(x[i])
            oy.append(y[i])
        x = np.linspace(4000, 4000, 500)
        y = np.linspace(1000, 2900, 500)
        for i in range(len(x)):
            ox.append(x[i])
            oy.append(y[i])
        x = np.linspace(4200, 5200, 500)
        y = np.linspace(4300, 4300, 500)
        for i in range(len(x)):
            ox.append(x[i])
            oy.append(y[i])
        C.XY_RESO = C.XY_RESO - 200
    if park == '16':
        x = np.linspace(5800, 1500, 500)
        y = np.linspace(2000, 2000, 500)
        for i in range(len(x)):
            ox.append(x[i])
            oy.append(y[i])
        x = np.linspace(8300, 8300, 500)
        y = np.linspace(2800, 500, 500)
        for i in range(len(x)):
            ox.append(x[i])
            oy.append(y[i])
        x = np.linspace(6900, 5200, 500)
        y = np.linspace(3600, 3600, 500)
        for i in range(len(x)):
            ox.append(x[i])
            oy.append(y[i])
        C.XY_RESO = C.XY_RESO - 200
    if park == '17':  # TODO
        pass
    if park == '18':  # TODO
        pass
    if park == '19'or park == '20'or park == '21'or park == '22'or park == '23'or park == '24'or park == '25':
        x = np.linspace(8300, 8300, 500)
        y = np.linspace(2800, 500, 500)
        for i in range(len(x)):
            ox.append(x[i])
            oy.append(y[i])
        x = np.linspace(1500, 5100, 500)
        y = np.linspace(2900, 2900, 500)
        for i in range(len(x)):
            ox.append(x[i])
            oy.append(y[i])
        x = np.linspace(1500, 3000, 500)
        y = np.linspace(2200, 2200, 500)
        for i in range(len(x)):
            ox.append(x[i])
            oy.append(y[i])
        x = np.linspace(5300, 3000, 500)
        y = np.linspace(2800, 2200, 500)
        for i in range(len(x)):
            ox.append(x[i])
            oy.append(y[i])
        C.XY_RESO = C.XY_RESO - 200
    if park == '26' or park == '27' or park == '28' or park == '29': # TODO
        pass
    if park == '30' or park == '31' or park == '32' or park == '33' or park == '34' or park == '35':
        x = np.linspace(10000, 7000, 500)
        y = np.linspace(2000, 2000, 500)
        for i in range(len(x)):
            ox.append(x[i])
            oy.append(y[i])
    if park == '36' or park == '37' or park == '38' or park == '39' or park == '40':
        x = np.linspace(10000, 7000, 500)
        y = np.linspace(2000, 2000, 500)
        for i in range(len(x)):
            ox.append(x[i])
            oy.append(y[i])
        x = np.linspace(12000, 14000, 500)
        y = np.linspace(860, 860, 500)
        for i in range(len(x)):
            ox.append(x[i])
            oy.append(y[i])

    if park == '41' or park == '42' or park == '43':  # TODO
        pass

    if park == '44' or park == '45' or park == '46'or park == '47'or park == '48'or park == '49':
        x = np.linspace(9000, 9000, 500)
        y = np.linspace(2800, 7000, 500)
        for i in range(len(x)):
            ox.append(x[i])
            oy.append(y[i])
        x = np.linspace(7200, 8800, 500)
        y = np.linspace(2000, 2800, 500)
        for i in range(len(x)):
            ox.append(x[i])
            oy.append(y[i])
        x = np.linspace(8500, 10200, 500)
        y = np.linspace(940, 1800, 500)
        for i in range(len(x)):
            ox.append(x[i])
            oy.append(y[i])
        x = np.linspace(9000, 10500, 500)
        y = np.linspace(6300, 6300, 500)
        for i in range(len(x)):
            ox.append(x[i])
            oy.append(y[i])
        C.XY_RESO =  C.XY_RESO - 200
    if park == '50':
        x = np.linspace(9000, 9000, 500)
        y = np.linspace(2800, 7000, 500)
        for i in range(len(x)):
            ox.append(x[i])
            oy.append(y[i])
        x = np.linspace(7200, 8800, 500)
        y = np.linspace(2000, 2800, 500)
        for i in range(len(x)):
            ox.append(x[i])
            oy.append(y[i])
        x = np.linspace(8500, 10200, 500)
        y = np.linspace(940, 1800, 500)
        for i in range(len(x)):
            ox.append(x[i])
            oy.append(y[i])
        x = np.linspace(9000, 10500, 500)
        y = np.linspace(6300, 6300, 500)
        for i in range(len(x)):
            ox.append(x[i])
            oy.append(y[i])
        C.XY_RESO =  C.XY_RESO - 500
        x = np.linspace(10100, 10100, 500)
        y = np.linspace(2500, 4500, 500)
        for i in range(len(x)):
            ox.append(x[i])
            oy.append(y[i])
        x = np.linspace(8200, 8200, 500)
        y = np.linspace(510, 1000, 500)
        for i in range(len(x)):
            ox.append(x[i])
            oy.append(y[i])

    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)
    C.XY_RESO = 1000
    # 算法测试结果保存
    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()