onsite_parking/planner/test_run.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
import time
# sys.setrecursionlimit(3000)  # 将默认的递归深度修改为3000

def linear_interpolation(x1,x2,y1,y2,num):
    x = np.linspace(x1,x2,num)
    y = np.linspace(y1,y2,num)
    return x,y

def changeOxyB01(park,ox,oy,sx,sy,gx,gy):
  
  if park == '1':
      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])
  elif park == '2':
    for i in range(0, 2500, 1):
      ox.append(sx + 500)
      oy.append(i)
    for i in range(0, 6000, 1):
      ox.append(i)
      oy.append(2000)
    for i in range(2000, 6000, 1):
      ox.append(4500)
      oy.append(i)
    for i in range(5500, 7000, 1):
      ox.append(5500)
      oy.append(i)
  elif park == '3':
    for i in range(0, 2500, 1):
      ox.append(sx + 500)
      oy.append(i)
    for i in range(0, 6000, 1):
      ox.append(i)
      oy.append(2000)
    for i in range(2000, 6000, 1):
      ox.append(4500)
      oy.append(i)
    for i in range(5500, 7000, 1):
      ox.append(5500)
      oy.append(i)
  elif park == '4':
    for i in range(0, 2500, 1):
      ox.append(sx + 500)
      oy.append(i)
    for i in range(0, 6000, 1):
      ox.append(i)
      oy.append(2000)
    for i in range(2000, 6000, 1):
      ox.append(4500)
      oy.append(i)
    for i in range(5500, 7000, 1):
      ox.append(5500)
      oy.append(i)
  elif park == '5':
    for i in range(0, 2500, 1):
      ox.append(sx + 500)
      oy.append(i)
    for i in range(0, 6000, 1):
      ox.append(i)
      oy.append(2000)
    for i in range(2000, 6000, 1):
      ox.append(4500)
      oy.append(i)
    for i in range(5500, 7000, 1):
      ox.append(5500)
      oy.append(i)
  elif park == '6':
    for i in range(0, 2500, 1):
      ox.append(sx + 500)
      oy.append(i)
    for i in range(0, 6000, 1):
      ox.append(i)
      oy.append(2000)
    for i in range(2000, 6000, 1):
      ox.append(4500)
      oy.append(i)
    for i in range(5500, 7000, 1):
      ox.append(5500)
      oy.append(i)
  elif park == '7':
    for i in range(0, 2500, 1):
      ox.append(sx + 500)
      oy.append(i)
    for i in range(0, 6000, 1):
      ox.append(i)
      oy.append(2000)
    for i in range(2000, 6000, 1):
      ox.append(4500)
      oy.append(i)
    for i in range(5500, 7000, 1):
      ox.append(5500)
      oy.append(i)
  elif park == '8':
    for i in range(0, 2500, 1):
      ox.append(sx + 500)
      oy.append(i)
    for i in range(0, 6000, 1):
      ox.append(i)
      oy.append(2000)
    for i in range(2000, 6000, 1):
      ox.append(4500)
      oy.append(i)
    for i in range(5500, 7000, 1):
      ox.append(5500)
      oy.append(i)
  elif park == '9':
    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])
  return ox,oy

def changeOxyB02(park,ox,oy,sx,sy,gx,gy):
  if park == '1':
    for i in range(sy - 500, sy + 1500, 1):
        ox.append(sx + 500)
        oy.append(i)
    for i in range(sy - 5500, sy - 500, 1):
        ox.append(sx - 1500)
        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 park == '2':
    for i in range(sy - 500, sy + 1500, 1):
        ox.append(sx + 500)
        oy.append(i)
    for i in range(sy - 5500, sy - 500, 1):
        ox.append(sx - 1500)
        oy.append(i)
    for i in range(sx - 1500, sx, 1):
        ox.append(i)
        oy.append(sy + 750)
  elif park == '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 park == '4':# TODO list assignment index out of range
    pass
  elif park == '5':# TODO list assignment index out of range
    pass
  elif park == '6':# TODO list assignment index out of range
    pass
  elif park == '7':# TODO list assignment index out of range
    pass
  elif park == '8':# TODO list assignment index out of range
    pass
  elif park == '9':# TODO list assignment index out of range
    pass
  elif park == '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(sx - 1500, sx, 100):
      ox.append(i)
      oy.append(sy + 750)
  elif park == '11': # TODO bad
    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, 100):
    #   ox.append(sx - 1300)
    #   oy.append(i)
    # for i in range(sx - 1500, sx, 100):
    #   ox.append(i)
    #   oy.append(sy + 750)
  elif park == '12': # TODO not good
    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 - 200)
      oy.append(i)
    for i in range(sx - 1500, sx, 100):
      ox.append(i)
      oy.append(sy + 750)
    for i in range(sy - 4500, sy + 1500, 100):
      ox.append(sx - 1500)
      oy.append(i)
  elif park == '13': # TODO bad
    for i in range(sy - 500, sy + 1500, 100):
      ox.append(sx + 500)
      oy.append(i)
    for i in range(sy - 5500, sy - 500, 100):
      ox.append(sx - 3200)
      oy.append(i)
    for i in range(sx - 1500, sx, 100):
      ox.append(i)
      oy.append(sy + 750)
    for i in range(sx - 1500, sx + 500, 100):
      ox.append(i)
      oy.append(sy - 500) 
    # for i in range(sy - 500, sy + 500, 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 - 4500, sx, 100):
    #   ox.append(i)
    #   oy.append(sy + 700)
    # for i in range(sy - 3500, sy + 500, 100):
    #   ox.append(sx - 1500)
    #   oy.append(i)       
    # for i in range(sx - 1500, sx, 100):
    #   ox.append(i)
    #   oy.append(sy - 750)
    # for i in range(sx - 1500, sx, 100):
    #   ox.append(i)
    #   oy.append(sy - 3500)
    # for i in range(gy + 800, sy - 500, 100):
    #   ox.append(sx - 3300)
    #   oy.append(i)
    # for i in range(gy + 1000, sy - 500, 100):
    #   ox.append(sx - 4200)
    #   oy.append(i)
    # for i in range(gy, gy + 1500, 100):
    #   ox.append(sx - 1500)
    #   oy.append(i)
  elif park == '14': # TODO Searching failed
    for i in range(sy - 500, sy + 1500, 100):
      ox.append(sx + 500)
      oy.append(i)
    for i in range(sy - 5500, sy - 500, 100):
      ox.append(sx - 3200)
      oy.append(i)
    for i in range(sx - 1500, sx, 100):
      ox.append(i)
      oy.append(sy + 750)
    for i in range(sx - 1500, sx + 500, 100):
      ox.append(i)
      oy.append(sy - 500) 
  elif park == '15':
    for i in range(sy - 500, sy + 1500, 100):
      ox.append(sx + 500)
      oy.append(i)
    for i in range(sy - 5500, sy - 500, 100):
      ox.append(sx - 3200)
      oy.append(i)
    for i in range(sx - 1500, sx, 100):
      ox.append(i)
      oy.append(sy + 750)
    for i in range(sx - 1500, sx + 500, 100):
      ox.append(i)
      oy.append(sy - 500)  
  elif park == '16':
    for i in range(sy - 500, sy + 1500, 100):
      ox.append(sx + 500)
      oy.append(i)
    for i in range(sy - 5500, sy - 500, 100):
      ox.append(sx - 3200)
      oy.append(i)
    for i in range(sx - 1500, sx, 100):
      ox.append(i)
      oy.append(sy + 750)
    for i in range(sx - 1500, sx + 500, 100):
      ox.append(i)
      oy.append(sy - 500)  
  elif park == '17':
    for i in range(sy - 500, sy + 1500, 100):
      ox.append(sx + 500)
      oy.append(i)
    for i in range(sy - 5500, sy - 500, 100):
      ox.append(sx - 3200)
      oy.append(i)
    for i in range(sx - 1500, sx, 100):
      ox.append(i)
      oy.append(sy + 750)
    for i in range(sx - 1500, sx + 500, 100):
      ox.append(i)
      oy.append(sy - 500)     
  elif park == '18':
    for i in range(sy - 500, sy + 1500, 100):
      ox.append(sx + 500)
      oy.append(i)
    for i in range(sy - 5500, sy - 500, 100):
      ox.append(sx - 3200)
      oy.append(i)
    for i in range(sx - 1500, sx, 100):
      ox.append(i)
      oy.append(sy + 750)
    for i in range(sx - 1500, sx + 500, 100):
      ox.append(i)
      oy.append(sy - 500)    
  elif park == '19':
    for i in range(sy - 500, sy + 1500, 100):
      ox.append(sx + 500)
      oy.append(i)
    for i in range(sy - 5500, sy - 500, 100):
      ox.append(sx - 3200)
      oy.append(i)
    for i in range(sx - 1500, sx, 100):
      ox.append(i)
      oy.append(sy + 750)
    for i in range(sx - 1500, sx + 500, 100):
      ox.append(i)
      oy.append(sy - 500)  
  elif park == '20':
    for i in range(sy - 500, sy + 1500, 100):
      ox.append(sx + 500)
      oy.append(i)
    for i in range(sy - 5500, sy - 500, 100):
      ox.append(sx - 3200)
      oy.append(i)
    for i in range(sx - 1500, sx, 100):
      ox.append(i)
      oy.append(sy + 750)
    for i in range(sx - 1500, sx + 500, 100):
      ox.append(i)
      oy.append(sy - 500)  
  elif park == '21': # TODO bad
    for i in range(sy - 500, sy + 1500, 100):
      ox.append(sx + 500)
      oy.append(i)
    for i in range(sy - 5500, sy - 500, 100):
      ox.append(sx - 3200)
      oy.append(i)
    for i in range(sx - 1500, sx, 100):
      ox.append(i)
      oy.append(sy + 700)
    for i in range(sx - 1500, sx + 500, 100):
      ox.append(i)
      oy.append(sy - 500)  
  elif park == '22':# TODO bad
    for i in range(sy - 500, sy + 1500, 100):
      ox.append(sx + 500)
      oy.append(i)
    for i in range(sy - 5500, sy - 500, 100):
      ox.append(sx - 3200)
      oy.append(i)
    for i in range(sx - 1500, sx, 100):
      ox.append(i)
      oy.append(sy + 750)
    for i in range(sx - 1500, sx + 500, 100):
      ox.append(i)
      oy.append(sy - 500)  
  elif park == '23':# TODO bad
    for i in range(sy - 500, sy + 1500, 100):
      ox.append(sx + 500)
      oy.append(i)
    for i in range(sy - 5500, sy - 500, 100):
      ox.append(sx - 3200)
      oy.append(i)
    for i in range(sx - 1500, sx, 100):
      ox.append(i)
      oy.append(sy + 750)
    for i in range(sx - 1500, sx + 500, 100):
      ox.append(i)
      oy.append(sy - 500)  
  elif park == '24':# TODO bad
    for i in range(sy - 500, sy + 1500, 100):
      ox.append(sx + 500)
      oy.append(i)
    for i in range(sy - 5500, sy - 500, 100):
      ox.append(sx - 3200)
      oy.append(i)
    for i in range(sx - 1500, sx, 100):
      ox.append(i)
      oy.append(sy + 750)
    for i in range(sx - 1500, sx + 500, 100):
      ox.append(i)
      oy.append(sy - 500)  
  elif park == '25':
    for i in range(sy - 500, sy + 1500, 100):
      ox.append(sx + 500)
      oy.append(i)
    for i in range(sy - 5500, sy - 500, 100):
      ox.append(sx - 3200)
      oy.append(i)
    for i in range(sx - 1500, sx, 100):
      ox.append(i)
      oy.append(sy + 750)
    for i in range(sx - 1500, sx + 500, 100):
      ox.append(i)
      oy.append(sy - 500)  
  elif park == '26':
    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)
  elif park == '27': # TODO list assignment index out of range
    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)
  elif park == '28': # TODO list assignment index out of range
    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)
  elif park == '29': # TODO list assignment index out of range
    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)
  elif park == '30': # TODO bad
    for i in range(sy - 500, sy + 1500, 100):
        ox.append(sx + 500)
        oy.append(i)
    for i in range(sx - 3500, sx, 100):
        ox.append(i)
        oy.append(sy + 600)
    for i in range(sx - 1500, sx + 500, 100):
      ox.append(i)
      oy.append(sy - 500)   
  elif park == '31': # TODO bad
    for i in range(sy - 500, sy + 1500, 100):
        ox.append(sx + 500)
        oy.append(i)
    for i in range(sx - 3500, sx, 100):
        ox.append(i)
        oy.append(sy + 600)
    for i in range(sx - 1500, sx + 500, 100):
      ox.append(i)
      oy.append(sy - 500)   
  elif park == '32': # TODO bad
    for i in range(sy - 500, sy + 1500, 100):
        ox.append(sx + 500)
        oy.append(i)
    for i in range(sx - 3500, sx, 100):
        ox.append(i)
        oy.append(sy + 600)
    for i in range(sx - 1500, sx + 500, 100):
      ox.append(i)
      oy.append(sy - 500) 
  elif park == '33': # TODO bad
    for i in range(sy - 500, sy + 1500, 100):
        ox.append(sx + 500)
        oy.append(i)
    for i in range(sx - 3500, sx, 100):
        ox.append(i)
        oy.append(sy + 600)
    for i in range(sx - 1500, sx + 500, 100):
      ox.append(i)
      oy.append(sy - 500)
  elif park == '34': # TODO bad
    for i in range(sy - 500, sy + 1500, 100):
        ox.append(sx + 500)
        oy.append(i)
    for i in range(sx - 3500, sx, 100):
        ox.append(i)
        oy.append(sy + 600)
    for i in range(sx - 1500, sx + 500, 100):
      ox.append(i)
      oy.append(sy - 500)
  elif park == '35': # TODO bad
    for i in range(sy - 500, sy + 1500, 100):
        ox.append(sx + 500)
        oy.append(i)
    for i in range(sx - 3500, sx, 100):
        ox.append(i)
        oy.append(sy + 600)
    for i in range(sx - 1500, sx + 500, 100):
      ox.append(i)
      oy.append(sy - 500)
    for i in range(sy - 5500, sy - 1500, 100):
      ox.append(sx - 4300)
      oy.append(i)
  elif park == '36': # TODO bad
    for i in range(sy - 500, sy + 1500, 100):
        ox.append(sx + 500)
        oy.append(i)
    for i in range(sx - 4500, sx, 100):
        ox.append(i)
        oy.append(sy + 700)
    for i in range(sx - 1500, sx + 500, 100):
      ox.append(i)
      oy.append(sy - 500)
  elif park == '37': # TODO bad
    for i in range(sy - 500, sy + 1500, 100):
        ox.append(sx + 500)
        oy.append(i)
    for i in range(sx - 3500, sx, 100):
        ox.append(i)
        oy.append(sy + 600)
    for i in range(sx - 1500, sx + 500, 100):
      ox.append(i)
      oy.append(sy - 500)
  elif park == '38': # TODO bad
    for i in range(sy - 500, sy + 1500, 100):
        ox.append(sx + 500)
        oy.append(i)
    for i in range(sx - 3500, sx, 100):
        ox.append(i)
        oy.append(sy + 600)
    for i in range(sx - 1500, sx + 500, 100):
      ox.append(i)
      oy.append(sy - 500)
  elif park == '39':
      for i in range(sy - 500, sy + 500, 1):
          ox.append(sx - 1500)
          oy.append(i)

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

      for i in range(sy - 3000, sy - 200, 1):
          ox.append(sx + 100)
          oy.append(i)
  elif park == '40':
      for i in range(sy - 500, sy + 500, 1):
          ox.append(sx - 1500)
          oy.append(i)

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

      for i in range(sy - 3000, sy - 200, 1):
          ox.append(sx + 100)
          oy.append(i)
  elif park == '41':
      for i in range(sy - 500, sy + 500, 1):
          ox.append(sx - 1500)
          oy.append(i)

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

      for i in range(sy - 3000, sy - 200, 1):
          ox.append(sx + 100)
          oy.append(i)
  elif park == '42':
      for i in range(sy - 500, sy + 500, 1):
          ox.append(sx - 1500)
          oy.append(i)

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

      for i in range(sy - 3000, sy - 200, 1):
          ox.append(sx + 100)
          oy.append(i)
  elif park == '43':
      for i in range(sy - 500, sy + 500, 1):
          ox.append(sx - 1500)
          oy.append(i)

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

      for i in range(sy - 3000, sy - 200, 1):
          ox.append(sx + 100)
          oy.append(i)
  elif park == '44':
      for i in range(sy - 500, sy + 500, 1):
          ox.append(sx - 1500)
          oy.append(i)

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

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

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

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

      for i in range(sx - 1500, sx, 1):
          ox.append(i)
          oy.append(sy + 300)
      
      # for i in range(sy - 1500, sy + 1500, 1):
      #     ox.append(sx + 100)
      #     oy.append(i)
  elif park == '47':
    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 + 300)

    for i in range(sx - 1500, sx, 1):
        ox.append(i)
        oy.append(sy - 1000)
  return ox,oy



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 = '17'
    gx, gy, gyaw0 = gp[park]['x_end'], gp[park]['y_end'], gp[park]['yaw']


    # print(type(ox),type(oy))
    # if map_path.split('/')[-1] == 'B01.json':
    #   ox,oy = changeOxyB01(park,ox,oy,sx,sy,gx,gy)
    # elif map_path.split('/')[-1] == 'B02.json':
    #   ox,oy = changeOxyB02(park,ox,oy,sx,sy,gx,gy)
    #   print(1)
    x = np.linspace(5800, 1500, 500)
    y = np.linspace(2800, 2800, 500)
    for i in range(len(x)):
        ox.append(x[i])
        oy.append(y[i])
    x = np.linspace(6000, 2000, 500)
    y = np.linspace(2000, 500, 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 - 900
    plt.plot(ox, oy, ",k")
    plt.show()

    start = time.time()
    # 规划算法
    path = planner.hybrid_astar_planning(sx, sy, syaw0, gx, gy, gyaw0, ox, oy, C.XY_RESO, C.YAW_RESO)
    end = time.time()
    print(f"Time: {end-start}s")
    # 算法测试结果保存
    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()