version1:finish 137 park

.gitignore

@@ -1,4 +1,4 @@
 *.jpg
 *.exe
-
+*.json
 input/

.idea/.gitignore

@@ -0,0 +1,8 @@
+# Default ignored files
+/shelf/
+/workspace.xml
+# Editor-based HTTP Client requests
+/httpRequests/
+# Datasource local storage ignored files
+/dataSources/
+/dataSources.local.xml

.idea/Onsite_Parking.iml

@@ -0,0 +1,12 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<module type="PYTHON_MODULE" version="4">
+  <component name="NewModuleRootManager">
+    <content url="file://$MODULE_DIR$" />
+    <orderEntry type="jdk" jdkName="Python 3.8 (Onsite_Parking)" jdkType="Python SDK" />
+    <orderEntry type="sourceFolder" forTests="false" />
+  </component>
+  <component name="PyDocumentationSettings">
+    <option name="format" value="PLAIN" />
+    <option name="myDocStringFormat" value="Plain" />
+  </component>
+</module>

.idea/inspectionProfiles/profiles_settings.xml

@@ -0,0 +1,6 @@
+<component name="InspectionProjectProfileManager">
+  <settings>
+    <option name="USE_PROJECT_PROFILE" value="false" />
+    <version value="1.0" />
+  </settings>
+</component>

.idea/misc.xml

@@ -0,0 +1,4 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<project version="4">
+  <component name="ProjectRootManager" version="2" project-jdk-name="Python 3.8 (Onsite_Parking)" project-jdk-type="Python SDK" />
+</project>

.idea/modules.xml

@@ -0,0 +1,8 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<project version="4">
+  <component name="ProjectModuleManager">
+    <modules>
+      <module fileurl="file://$PROJECT_DIR$/.idea/Onsite_Parking.iml" filepath="$PROJECT_DIR$/.idea/Onsite_Parking.iml" />
+    </modules>
+  </component>
+</project>

.idea/vcs.xml

@@ -0,0 +1,6 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<project version="4">
+  <component name="VcsDirectoryMappings">
+    <mapping directory="" vcs="Git" />
+  </component>
+</project>

planner/hybridastar/astar.py

@@ -129,8 +129,9 @@ def calc_holonomic_heuristic_with_obstacle(node, ox, oy, reso, rr):
 
     hmap = [[np.inf for _ in range(P.yw)] for _ in range(P.xw)]
 
+
     for n in closed_set.values():
-        hmap[n.x - P.minx][n.y - P.miny] = n.cost
+      hmap[n.x - P.minx][n.y - P.miny] = n.cost
 
     return hmap
 

planner/run_test_b01.py

@@ -0,0 +1,241 @@
+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()

planner/run_test_b03.py

@@ -0,0 +1,551 @@
+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()

planner/test_run.py

@@ -10,28 +10,751 @@ 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/Atest.json'
-    mdp.map_display(map_path) #  仅绘制地图
+    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 = '3'
+    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!")
@@ -42,11 +765,12 @@ def main():
         "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:
+    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"
+    result_path = f"./output/result_{map_path.split('/')[-1].split('.json')[0]}_{park}.json"
     replay.replay(map_path, result_path)
+
 if __name__ == '__main__':
     main()

planner/user_run_new.py

@@ -0,0 +1,109 @@
+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()