onsite_parking/planner/test_run.py

import os
import sys
# sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
#                 "/../../onsite_parking/")
sys.path.insert(0, "/home/yct/Documents/data/onsite/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(C_,park,ori_ox,ori_oy):
  ox = ori_ox.copy()
  oy = ori_oy.copy()
  C = C_
  isSearchFailed = False

  if 1<=int(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(9500, 9500, 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(5500, 6000, 500)
      for i in range(len(x)):
          ox.append(x[i])
          oy.append(y[i])

      x = np.linspace(4500, 8800, 500)
      y = np.linspace(6000, 6000, 500)
      for i in range(len(x)):
          ox.append(x[i])
          oy.append(y[i])

      x = np.linspace(0, 9500, 500)
      y = np.linspace(6700, 6700, 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])
      
      C.XY_RESO = C.XY_RESO - 600

  if 9<=int(park)<=10:
      x = np.linspace(8200, 8200, 500)
      y = np.linspace(7000, 0, 500)
      for i in range(len(x)):
          ox.append(x[i])
          oy.append(y[i])

      x = np.linspace(1500, 5800, 500)
      y = np.linspace(2000, 2000, 500)
      for i in range(len(x)):
          ox.append(x[i])
          oy.append(y[i])

      x = np.linspace(2000, 2000, 500)
      y = np.linspace(4200, 2800, 500)
      for i in range(len(x)):
          ox.append(x[i])
          oy.append(y[i])

      x = np.linspace(2000, 5400, 500)
      y = np.linspace(2800, 2800, 500)
      for i in range(len(x)):
          ox.append(x[i])
          oy.append(y[i])
      C.XY_RESO = C.XY_RESO - 500

  if 11<=int(park)<=12:
      x = np.linspace(8200, 8200, 500)
      y = np.linspace(7000, 0, 500)
      for i in range(len(x)):
          ox.append(x[i])
          oy.append(y[i])

      x = np.linspace(1500, 5800, 500)
      y = np.linspace(2000, 2000, 500)
      for i in range(len(x)):
          ox.append(x[i])
          oy.append(y[i])

      x = np.linspace(1500, 1500, 500)
      y = np.linspace(2000, 4200, 500)
      for i in range(len(x)):
          ox.append(x[i])
          oy.append(y[i])

      x = np.linspace(2800, 2800, 500)
      y = np.linspace(2800, 4200, 500)
      for i in range(len(x)):
          ox.append(x[i])
          oy.append(y[i])

      x = np.linspace(2800, 5800, 500)
      y = np.linspace(2800, 2800, 500)
      for i in range(len(x)):
          ox.append(x[i])
          oy.append(y[i])

      C.XY_RESO = C.XY_RESO - 400

  if 13<=int(park)<=15:
      x = np.linspace(8200, 8200, 500)
      y = np.linspace(7000, 0, 500)
      for i in range(len(x)):
          ox.append(x[i])
          oy.append(y[i])

      x = np.linspace(1500, 5800, 500)
      y = np.linspace(2000, 2000, 500)
      for i in range(len(x)):
          ox.append(x[i])
          oy.append(y[i])

      x = np.linspace(2400, 2400, 500)
      y = np.linspace(2000, 4200, 500)
      for i in range(len(x)):
          ox.append(x[i])
          oy.append(y[i])

      x = np.linspace(4200, 4200, 500)
      y = np.linspace(2800, 4200, 500)
      for i in range(len(x)):
          ox.append(x[i])
          oy.append(y[i])

      x = np.linspace(4200, 5800, 500)
      y = np.linspace(2800, 2800, 500)
      for i in range(len(x)):
          ox.append(x[i])
          oy.append(y[i])

      C.XY_RESO = C.XY_RESO - 400

  if int(park)==16:
      x = np.linspace(8200, 8200, 500)
      y = np.linspace(7000, 0, 500)
      for i in range(len(x)):
          ox.append(x[i])
          oy.append(y[i])

      x = np.linspace(1500, 5800, 500)
      y = np.linspace(1800, 1800, 500)
      for i in range(len(x)):
          ox.append(x[i])
          oy.append(y[i])

      x = np.linspace(1500, 5800, 500)
      y = np.linspace(3200, 3200, 500)
      for i in range(len(x)):
          ox.append(x[i])
          oy.append(y[i])

      C.XY_RESO = C.XY_RESO - 300

  if park == '17':  # TODO
      x = np.linspace(8200, 8200, 500)
      y = np.linspace(7000, 0, 500)
      for i in range(len(x)):
          ox.append(x[i])
          oy.append(y[i])

      x = np.linspace(500, 1200, 500)
      y = np.linspace(1200, 680, 500)
      for i in range(len(x)):
          ox.append(x[i])
          oy.append(y[i])

      x = np.linspace(5500, 1200, 500)
      y = np.linspace(2800, 1800, 500)
      for i in range(len(x)):
          ox.append(x[i])
          oy.append(y[i])

      x = np.linspace(5800, 2000, 500)
      y = np.linspace(1800, 680, 500)
      for i in range(len(x)):
          ox.append(x[i])
          oy.append(y[i])

      x = np.linspace(1000, 2100, 500)
      y = np.linspace(700, 700, 500)
      for i in range(len(x)):
          ox.append(x[i])
          oy.append(y[i])

      C.XY_RESO = C.XY_RESO - 800

  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
      x = np.linspace(7200, 7200, 500)
      y = np.linspace(7000, 0, 500)
      for i in range(len(x)):
          ox.append(x[i])
          oy.append(y[i])

      x = np.linspace(9800, 9800, 500)
      y = np.linspace(4000, 0, 500)
      for i in range(len(x)):
          ox.append(x[i])
          oy.append(y[i])

      x = np.linspace(8000, 8000, 500)
      y = np.linspace(2000, 0, 500)
      for i in range(len(x)):
          ox.append(x[i])
          oy.append(y[i])

      x = np.linspace(8000, 9800, 500)
      y = np.linspace(2000, 2000, 500)
      for i in range(len(x)):
          ox.append(x[i])
          oy.append(y[i])


      C.XY_RESO = C.XY_RESO - 500

  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])

  return ox,oy,isSearchFailed


def changeOxyB02(C_,park,ori_ox,ori_oy):
  ox = ori_ox.copy()
  oy = ori_oy.copy()
  isSearchFailed = False
  C = C_
  if park == '1':
    C.XY_RESO =  C.XY_RESO - 200
    x = np.linspace(4000, 6000, 500)
    y = np.linspace(7000, 7000, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    x = np.linspace(6000, 6000, 500)
    y = np.linspace(6000, 7000, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    x = np.linspace(4000, 6000, 500)
    y = np.linspace(4000, 4000, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    x = np.linspace(4000, 4000, 500)
    y = np.linspace(5000, 7000, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
  elif park == '2':
    C.XY_RESO =  C.XY_RESO - 800
    x = np.linspace(4000, 6000, 500)
    y = np.linspace(7000, 7000, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    x = np.linspace(6000, 6000, 500)
    y = np.linspace(6000, 7000, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    x = np.linspace(4000, 6000, 500)
    y = np.linspace(4500, 4500, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    x = np.linspace(4000, 4000, 500)
    y = np.linspace(5000, 7000, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
  elif park == '3':
    C.XY_RESO =  C.XY_RESO - 800
    x = np.linspace(4000, 6000, 500)
    y = np.linspace(7000, 7000, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    x = np.linspace(6000, 6000, 500)
    y = np.linspace(6000, 7000, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    x = np.linspace(4000, 6000, 500)
    y = np.linspace(4000, 4000, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    x = np.linspace(4000, 4000, 500)
    y = np.linspace(5000, 7000, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])   
  elif park == '4':
    C.XY_RESO =  C.XY_RESO - 800
    x = np.linspace(4000, 6000, 500)
    y = np.linspace(7000, 7000, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    x = np.linspace(6000, 6000, 500)
    y = np.linspace(6000, 7000, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    x = np.linspace(4000, 6000, 500)
    y = np.linspace(3500, 3500, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    x = np.linspace(4000, 4000, 500)
    y = np.linspace(5000, 7000, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])   
  elif park == '5':
    C.XY_RESO =  C.XY_RESO - 800
    x = np.linspace(4000, 6000, 500)
    y = np.linspace(7000, 7000, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    x = np.linspace(6000, 6000, 500)
    y = np.linspace(6000, 7000, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    x = np.linspace(4000, 6000, 500)
    y = np.linspace(3000, 3000, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    x = np.linspace(4000, 4000, 500)
    y = np.linspace(5000, 7000, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])   
  elif park == '6':
    C.XY_RESO =  C.XY_RESO - 800
    x = np.linspace(4000, 6000, 500)
    y = np.linspace(7000, 7000, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    x = np.linspace(6000, 6000, 500)
    y = np.linspace(6000, 7000, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    x = np.linspace(4000, 6000, 500)
    y = np.linspace(1500, 1500, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    x = np.linspace(4000, 4000, 500)
    y = np.linspace(1500, 7000, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])   
  elif park == '7':
    C.XY_RESO =  C.XY_RESO - 800
    x = np.linspace(4000, 6000, 500)
    y = np.linspace(7000, 7000, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    x = np.linspace(6000, 6000, 500)
    y = np.linspace(6000, 7000, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    x = np.linspace(4000, 6000, 500)
    y = np.linspace(1000, 1000, 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, 7000, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])   
  elif park == '8':
    C.XY_RESO =  C.XY_RESO - 800
    x = np.linspace(4000, 6000, 500)
    y = np.linspace(7000, 7000, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    x = np.linspace(6000, 6000, 500)
    y = np.linspace(6000, 7000, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    # x = np.linspace(4000, 6000, 500)
    # y = np.linspace(1000, 1000, 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, 7000, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])   
  elif park == '9':
    C.XY_RESO =  C.XY_RESO - 800
    x = np.linspace(4000, 6000, 500)
    y = np.linspace(7000, 7000, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    x = np.linspace(5500, 5500, 500)
    y = np.linspace(6000, 7000, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    x = np.linspace(5000, 6000, 500)
    y = np.linspace(6000, 6000, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    x = np.linspace(4500, 4500, 500)
    y = np.linspace(0, 7000, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])   
    x = np.linspace(5000, 5000, 500)
    y = np.linspace(1000, 6000, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])  
  elif park == '10':
    C.XY_RESO =  C.XY_RESO - 800
    x = np.linspace(4000, 6000, 500)
    y = np.linspace(7000, 7000, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    x = np.linspace(5500, 5500, 500)
    y = np.linspace(6000, 7000, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    x = np.linspace(5000, 6000, 500)
    y = np.linspace(6000, 6000, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
  elif park == '11':
    C.XY_RESO =  C.XY_RESO - 800
    x = np.linspace(4000, 6000, 500)
    y = np.linspace(7000, 7000, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    x = np.linspace(5500, 5500, 500)
    y = np.linspace(6000, 7000, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    x = np.linspace(5000, 6000, 500)
    y = np.linspace(6000, 6000, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    x = np.linspace(4500, 4500, 500)
    y = np.linspace(1500, 7000, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    x = np.linspace(5000, 5000, 500)
    y = np.linspace(0, 6000, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    x = np.linspace(3000, 4500, 500)
    y = np.linspace(1500, 1500, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
  elif park == '12':
    C.XY_RESO =  C.XY_RESO - 800
    x = np.linspace(4000, 6000, 500)
    y = np.linspace(7000, 7000, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    x = np.linspace(5500, 5500, 500)
    y = np.linspace(6000, 7000, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    x = np.linspace(5000, 6000, 500)
    y = np.linspace(6000, 6000, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    x = np.linspace(4500, 4500, 500)
    y = np.linspace(1000, 7000, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    x = np.linspace(5000, 5000, 500)
    y = np.linspace(0, 6000, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    x = np.linspace(3000, 4500, 500)
    y = np.linspace(1000, 1000, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
  elif park == '13':
    C.XY_RESO =  C.XY_RESO - 800
    x = np.linspace(4000, 6000, 500)
    y = np.linspace(7000, 7000, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    x = np.linspace(5500, 5500, 500)
    y = np.linspace(6000, 7000, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    x = np.linspace(5000, 6000, 500)
    y = np.linspace(6000, 6000, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    x = np.linspace(4500, 4500, 500)
    y = np.linspace(1000, 7000, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    x = np.linspace(5000, 5000, 500)
    y = np.linspace(0, 6000, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    x = np.linspace(2000, 4500, 500)
    y = np.linspace(1000, 1000, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    x = np.linspace(3000, 5000, 500)
    y = np.linspace(300, 300, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
  elif park == '14':
    C.XY_RESO =  C.XY_RESO - 800
    x = np.linspace(4000, 6000, 500)
    y = np.linspace(7000, 7000, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    x = np.linspace(5500, 5500, 500)
    y = np.linspace(6000, 7000, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    x = np.linspace(5000, 6000, 500)
    y = np.linspace(6000, 6000, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    x = np.linspace(4500, 4500, 500)
    y = np.linspace(1000, 7000, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    x = np.linspace(5000, 5000, 500)
    y = np.linspace(0, 6000, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    x = np.linspace(0, 4500, 500)
    y = np.linspace(1000, 1000, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    x = np.linspace(2000, 5000, 500)
    y = np.linspace(300, 300, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
  elif park == '15' or park == '16' or park == '17' or park == '18' or park == '19' or park == '20' or park == '21' or park == '22':
    C.XY_RESO =  C.XY_RESO - 200
    x = np.linspace(5500, 5500, 500)
    y = np.linspace(6200, 7000, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    x = np.linspace(1800, 1800, 500)
    y = np.linspace(1000, 6200, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])     
    x = np.linspace(500, 5000, 500)
    y = np.linspace(7300, 7300, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    x = np.linspace(3500, 5500, 500)
    y = np.linspace(6200, 6200, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
  elif park == '23' or park == '24': # TOD: 23,24
    C.XY_RESO =  C.XY_RESO - 300
    x = np.linspace(5500, 5500, 500)
    y = np.linspace(6200, 7000, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    x = np.linspace(1800, 1800, 500)
    y = np.linspace(1000, 6200, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])     
    x = np.linspace(500, 5000, 500)
    y = np.linspace(7300, 7300, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    x = np.linspace(3500, 5500, 500)
    y = np.linspace(6200, 6200, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])     
  elif park =='25':
    C.XY_RESO =  C.XY_RESO - 300
    x = np.linspace(5500, 5500, 500)
    y = np.linspace(6200, 7000, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    x = np.linspace(1800, 1800, 500)
    y = np.linspace(1000, 6200, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])     
    x = np.linspace(500, 5000, 500)
    y = np.linspace(7300, 7300, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    x = np.linspace(3500, 5500, 500)
    y = np.linspace(6200, 6200, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    x = np.linspace(500, 2500, 500)
    y = np.linspace(5500, 5500, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])       
  elif park == '26':
    C.XY_RESO =  C.XY_RESO - 200
    x = np.linspace(5500, 5500, 500)
    y = np.linspace(6200, 7000, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    x = np.linspace(1800, 1800, 500)
    y = np.linspace(1000, 6200, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])     
    x = np.linspace(500, 5000, 500)
    y = np.linspace(7300, 7300, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    x = np.linspace(3500, 5500, 500)
    y = np.linspace(6200, 6200, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
  elif park == '27':
    C.XY_RESO =  C.XY_RESO - 300
    x = np.linspace(5500, 5500, 500)
    y = np.linspace(6200, 7000, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])   
    x = np.linspace(1500, 5000, 500)
    y = np.linspace(7300, 7300, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    x = np.linspace(3500, 5500, 500)
    y = np.linspace(6200, 6200, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
  elif park == '28':
    C.XY_RESO =  C.XY_RESO - 300
    x = np.linspace(5500, 5500, 500)
    y = np.linspace(6200, 7000, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])   
    x = np.linspace(3500, 5000, 500)
    y = np.linspace(7300, 7300, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    x = np.linspace(3500, 5500, 500)
    y = np.linspace(6200, 6200, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
  elif park == '29': # TOD: 29
    C.XY_RESO =  C.XY_RESO - 300
    x = np.linspace(5500, 5500, 500)
    y = np.linspace(6200, 7000, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])   
    x = np.linspace(4000, 5000, 500)
    y = np.linspace(7300, 7300, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    x = np.linspace(3500, 5500, 500)
    y = np.linspace(6200, 6200, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
  elif park == '30':
    C.XY_RESO =  C.XY_RESO - 200
    x = np.linspace(5500, 5500, 500)
    y = np.linspace(6200, 7000, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])   
    x = np.linspace(500, 5000, 500)
    y = np.linspace(7300, 7300, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    x = np.linspace(3500, 5500, 500)
    y = np.linspace(6200, 6200, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
  elif park == '31':
    C.XY_RESO =  C.XY_RESO - 200
    x = np.linspace(5500, 5500, 500)
    y = np.linspace(6200, 7000, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])   
    x = np.linspace(500, 5000, 500)
    y = np.linspace(7300, 7300, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    x = np.linspace(3500, 5500, 500)
    y = np.linspace(6200, 6200, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    x = np.linspace(1000, 1000, 500)
    y = np.linspace(200, 7300, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i]) 
  elif park == '32':
    C.XY_RESO =  C.XY_RESO - 700
    x = np.linspace(5500, 5500, 500)
    y = np.linspace(6200, 7300, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])   
    x = np.linspace(500, 5500, 500)
    y = np.linspace(7200, 7200, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    x = np.linspace(3500, 5500, 500)
    y = np.linspace(6200, 6200, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    x = np.linspace(1200, 1200, 500)
    y = np.linspace(200, 7300, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])   
    x = np.linspace(1800, 1800, 500)
    y = np.linspace(5500, 6500, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])  
    x = np.linspace(500, 5500, 500)
    y = np.linspace(4200, 4200, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i]) 
  elif park == '33' or park == '34' or park == '36':
    C.XY_RESO =  C.XY_RESO - 200
    x = np.linspace(5500, 5500, 500)
    y = np.linspace(6200, 7000, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])   
    x = np.linspace(500, 5000, 500)
    y = np.linspace(7300, 7300, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    x = np.linspace(3500, 5500, 500)
    y = np.linspace(6200, 6200, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    x = np.linspace(1000, 1000, 500)
    y = np.linspace(200, 7300, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
  elif park == '35':
    C.XY_RESO =  C.XY_RESO - 700
    x = np.linspace(5500, 5500, 500)
    y = np.linspace(6200, 7300, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])   
    x = np.linspace(500, 5500, 500)
    y = np.linspace(7200, 7200, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    x = np.linspace(3500, 5500, 500)
    y = np.linspace(6200, 6200, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    x = np.linspace(1200, 1200, 500)
    y = np.linspace(200, 7300, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])   
    x = np.linspace(1800, 1800, 500)
    y = np.linspace(4500, 6500, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])  
    x = np.linspace(500, 5500, 500)
    y = np.linspace(1500, 1500, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i]) 
  elif park == '37':
    C.XY_RESO =  C.XY_RESO - 700
    x = np.linspace(5500, 5500, 500)
    y = np.linspace(6200, 7300, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])   
    x = np.linspace(500, 5500, 500)
    y = np.linspace(7200, 7200, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    x = np.linspace(3500, 5500, 500)
    y = np.linspace(6200, 6200, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    x = np.linspace(1200, 1200, 500)
    y = np.linspace(200, 7300, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])   
    x = np.linspace(1800, 1800, 500)
    y = np.linspace(2500, 6500, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])  
    x = np.linspace(500, 5500, 500)
    y = np.linspace(500, 500, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
  elif park == '38':# TODO 35 37 38 39
    C.XY_RESO =  C.XY_RESO - 700
    x = np.linspace(5500, 5500, 500)
    y = np.linspace(6200, 7300, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])   
    x = np.linspace(500, 5500, 500)
    y = np.linspace(7200, 7200, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    x = np.linspace(3500, 5500, 500)
    y = np.linspace(6200, 6200, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    x = np.linspace(1200, 1200, 500)
    y = np.linspace(200, 7300, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])   
    x = np.linspace(1800, 1800, 500)
    y = np.linspace(2500, 6500, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])  
    x = np.linspace(500, 5500, 500)
    y = np.linspace(500, 500, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])   
  elif park == '39' or park == '42' or park == '43' or park == '44':
    x = np.linspace(5500, 5500, 500)
    y = np.linspace(6200, 7300, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])   
    x = np.linspace(500, 5500, 500)
    y = np.linspace(7200, 7200, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])    
    x = np.linspace(4500, 4500, 500)
    y = np.linspace(500, 6200, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])  
    x = np.linspace(3500, 3500, 500)
    y = np.linspace(6200, 7300, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])    
  elif park == '40' or park == '41' or park == '45':
    x = np.linspace(3500, 3500, 500)
    y = np.linspace(6200, 7300, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])  
    x = np.linspace(3500, 5000, 500)
    y = np.linspace(7000, 7000, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])  
    x = np.linspace(5100, 5100, 500)
    y = np.linspace(3700, 6500, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])   
  elif park == '46' or park == '47':
    x = np.linspace(3500, 3500, 500)
    y = np.linspace(6200, 7300, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])  
    x = np.linspace(3500, 5000, 500)
    y = np.linspace(7000, 7000, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])  
    x = np.linspace(5100, 5100, 500)
    y = np.linspace(3700, 6500, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])   
    x = np.linspace(3500, 5000, 500)
    y = np.linspace(4500, 4500, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])       
  return ox,oy,isSearchFailed

def changeOxyB03(C_,park,ori_ox,ori_oy):
  ox = ori_ox.copy()
  oy = ori_oy.copy()
  isSearchFailed = False
  C = C_
  if park == '1' or park == '2' or park == '3' or park == '4' or park == '5' or park == '6' or park == '7' or park == '8' or park == '9' or park == '10' or park == '11' or park == '12' or park == '13' or park == '14' or park == '15' or park == '16': 
    C.XY_RESO =  C.XY_RESO - 200
    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])
  elif park == '17' or park == '18' or park == '19' or park == '20':
    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])
  elif park == '21' or park == '22' or park == '23':
      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])
  elif park == '24' or park == '25' or park == '26':
    C.XY_RESO =  C.XY_RESO - 800
    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(4500, 7000, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    x = np.linspace(3500, 4500, 500)
    y = np.linspace(4500, 4500, 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, 500)
    y = np.linspace(7700, 6600, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    x = np.linspace(1000, 3500, 500)
    y = np.linspace(3100, 3100, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
  elif park == '27':
     pass
  elif park == '28' or park == '29':
    C.XY_RESO =  C.XY_RESO - 800
    x = np.linspace(1000, 1000, 500)
    y = np.linspace(7700, 6600, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    x = np.linspace(1000, 5000, 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, 5000, 500)
    y = np.linspace(4500, 4500, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])  
    x = np.linspace(4500, 4500, 500)
    y = np.linspace(5000, 6000, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
  elif park == '30' or park == '31' or park == '32':
    C.XY_RESO =  C.XY_RESO - 900
    x = np.linspace(4500, 4500, 500)
    y = np.linspace(5000, 6000, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
    x = np.linspace(0, 4500, 500)
    y = np.linspace(5500, 5500, 500)
    for i in range(len(x)):
      ox.append(x[i])
      oy.append(y[i])
  if park == '62'or park == '61':
    for i in range(0, 3000, 1):
        ox.append(i)
        oy.append(5600)
    for i in range(1000, 4500, 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(0, 3000, 1):
          ox.append(i)
          oy.append(5600)
      for i in range(1000, 4500, 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(0, 3000, 1):
          ox.append(i)
          oy.append(5600)
      for i in range(1000, 4500, 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(0, 3000, 1):
          ox.append(i)
          oy.append(5600)
      for i in range(1000, 4500, 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(0, 3000, 1):
          ox.append(i)
          oy.append(5600)
      for i in range(1000, 4500, 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(0, 3000, 1):
          ox.append(i)
          oy.append(5600)
      for i in range(1000, 4500, 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(0, 3000, 1):
          ox.append(i)
          oy.append(5600)
      for i in range(1000, 4500, 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(0, 2200, 1):
          ox.append(i)
          oy.append(5600)
      for i in range(1000, 4500, 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(0, 2200, 1):
          ox.append(i)
          oy.append(5600)
      for i in range(1000, 4500, 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(0, 2200, 1):
          ox.append(i)
          oy.append(5600)
      for i in range(1000, 4500, 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(0, 2200, 1):
          ox.append(i)
          oy.append(5600)
      for i in range(1000, 4500, 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(0, 2200, 1):
          ox.append(i)
          oy.append(5600)
      for i in range(1000, 4500, 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(0, 2200, 1):
          ox.append(i)
          oy.append(5600)
      for i in range(1000, 4500, 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])


  return ox,oy,isSearchFailed

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']

    isSearchFailed = False
    ox, oy, isSearchFailed = changeOxyB01(C,park,ox,oy)

    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:    plt.plot(ox, oy, ",k")
    plt.show()
    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()