0530 init

.gitignore

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

.idea/.gitignore

@@ -1,8 +1,8 @@
-# Default ignored files
-/shelf/
-/workspace.xml
-# Editor-based HTTP Client requests
-/httpRequests/
-# Datasource local storage ignored files
-/dataSources/
-/dataSources.local.xml
+# 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

@@ -1,12 +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>
+<?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="C:\Users\LK\miniconda3" 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/misc.xml

@@ -1,4 +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" />
+<?xml version="1.0" encoding="UTF-8"?>
+<project version="4">
+  <component name="ProjectRootManager" version="2" project-jdk-name="C:\Users\LK\miniconda3" project-jdk-type="Python SDK" />
 </project>

.idea/modules.xml

@@ -1,8 +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>
+<?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

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

planner/run_test_b01.py

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

@@ -1,551 +0,0 @@
-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/user_run_new.py

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

utils/replay.py

@@ -68,3 +68,57 @@ def replay(map_scene,output_result):
     else:
         print("仿真结束!")
 
+# def replay(map_scene, output_result):
+#     picture_scene = map_scene.replace('/', '/').replace('.json', '.jpg')
+#     gif_scene = map_scene.replace('input', 'image_save').replace('.json', '.gif')
+#     if output_result[-6].isdigit():
+#         Num = output_result.rsplit('/result_', 1)[-1].rsplit('.', 1)[0]
+#         if Num[-2].isdigit():
+#             gif_scene = gif_scene.replace('.gif', f'{Num[-3:]}.gif')
+#         else:
+#             gif_scene = gif_scene.replace('.gif', f'{Num[-2:]}.gif')  
+#     C = make_car.C
+#     ox, oy, sp, gp = mp.make_map(map_scene)
+#     sx, sy, syaw0 = sp['x'], sp['y'], sp['yaw']
+#     gx, gy, gyaw0 = gp['3']['x_end'], gp['3']['y_end'], gp['3']['yaw']
+#     with open(output_result, 'r', encoding='UTF-8') as f:
+#         result = json.load(f)
+#     x = result['output_x']
+#     y = result['output_y']
+#     yaw = result['output_yaw']
+#     direction = result['output_dir']
+#     plt.rcParams['xtick.direction'] = 'in'
+#     picture = plt.imread(picture_scene)
+#     ox1, ox2, oy1, oy2 = min(ox), max(ox), min(oy), max(oy)
+#     frames = []  # 用于保存每一帧图像
+#     for k in range(len(x)):
+#         if k % 3 == 0:
+#             fig, ax = plt.subplots()
+#             ax.imshow(picture, extent=[ox1, ox2, oy1, oy2], aspect='auto')
+#             while ax.lines:
+#                 ax.lines[0].remove()
+#             ax.plot(x, y, linewidth=0.5, color='b', linestyle='--')
+#             if k < len(x) - 2:
+#                 dy = (yaw[k + 1] - yaw[k]) / C.MOVE_STEP
+#                 steer = rs.pi_2_pi(math.atan(-C.WB * dy / direction[k]))
+#             else:
+#                 steer = 0.0
+#             tools.draw_car(ax, gx, gy, gyaw0, 0.0, 'dimgray')
+#             tools.draw_car(ax, x[k], y[k], yaw[k], steer)
+#             ax.set_title("Simulation Result", loc='left', fontweight="heavy")
+#             ax.axis("equal")
+#             # 保存当前帧图像
+#             fig.canvas.draw()
+#             image = np.frombuffer(fig.canvas.tostring_rgb(), dtype='uint8')
+#             image = image.reshape(fig.canvas.get_width_height()[::-1] + (3,))
+#             frames.append(image)
+#             plt.close(fig)  # 关闭当前图像以避免超过限制
+#     # 将所有保存的帧图像整合成 GIF 动态图
+#     imageio.mimsave(gif_scene, frames, fps=15)
+#     if output_result[-6].isdigit():
+#         if Num[-2].isdigit():
+#             print(f"车位{Num[-2:]}仿真完成")
+#         else:
+#             print(f"车位{Num[-1]}仿真完成")
+#     else:
+#         print("仿真结束!")