onsite_parking/planner/user_run_new.py

import os
import sys
sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
                "/../../onsite_parking/")

import json
from planner.hybridastar import planner as planner
import input.make_map as mp
from input import make_car
from utils import replay
from utils import map_display as mdp
import matplotlib.pyplot as plt
sys.setrecursionlimit(3000)  # 将默认的递归深度修改为3000
def main():
    # 请在此输入input文件夹下场景文件
    map_path = './input/B02.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
    # 遍历所有停车位
    for i in range(1,len(gp)+1):
      if i == '1':
        for i in range(sy - 500, sy + 1500, 1):
            ox.append(sx + 500)
            oy.append(i)

        for i in range(sx - 1500, sx, 1):
            ox.append(i)
            oy.append(sy + 750)
        for i in range(sx - 1500, sx, 1):
            ox.append(i)
            oy.append(sy - 3500)
      elif i == '2':
        pass
      elif i == '3':
        for i in range(sy - 500, sy + 1500, 1):
            ox.append(sx + 500)
            oy.append(i)

        # for i in range(gy + 500, sy - 1500, 100):
        #     ox.append(sx)
        #     oy.append(i)

        # for i in range(sx - 500, sx, 100):
        #     ox.append(i)
        #     oy.append(sy - 750)

        for i in range(sx - 1500, sx, 1):
            ox.append(i)
            oy.append(sy + 750)
        for i in range(sx - 1500, sx, 1):
            ox.append(i)
            oy.append(sy - 3500)

      elif i == '10':
        for i in range(sy - 500, sy + 1500, 100):
            ox.append(sx + 500)
            oy.append(i)

        for i in range(gy + 500, sy - 1500, 100):
            ox.append(sx)
            oy.append(i)

        # for i in range(gy + 1500, sy + 500, 1):
        #     ox.append(sx - 1000)
        #     oy.append(i)

        # for i in range(sx - 500, sx, 100):
        #     ox.append(i)
        #     oy.append(sy - 750)

        for i in range(sx - 1500, sx, 100):
            ox.append(i)
            oy.append(sy + 750)
      elif i == '26':
        pass
            # for i in range(sy - 500, sy + 1500, 100):
            #     ox.append(sx + 500)
            #     oy.append(i)
            # for i in range(gy + 500, sy - 1500, 100):
            #     ox.append(sx)
            #     oy.append(i)
            # for i in range(sx - 2000, sx, 100):
            #     ox.append(i)
            #     oy.append(sy + 600)

      gx, gy, gyaw0 = gp[str(i)]['x_end'], gp[str(i)]['y_end'], gp[str(i)]['yaw']
      # 请在此调用planer文件夹下的规控算法
      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/use_result_{map_path.split('/')[-1].split('.json')[0]}_{str(i)}.json", "w") as file:
          json.dump(output_dit, file)

      # 仿真回放
      result_path = f"./output/use_result_{map_path.split('/')[-1].split('.json')[0]}_{str(i)}.json"
      replay.replay(map_path, result_path)

if __name__ == '__main__':
    main()