add save traj&pic

animation/animation.py

@@ -56,19 +56,22 @@ def plot_node(nodes, current_node):
         plt.draw()
 
     @staticmethod
-    def plot_path(x,y,color='grey'):
+    def plot_path(x,y,color='grey',label=None):
         plt.figure(1)
-        plt.plot(x,y,'*-',linewidth=1,color=color)
+        plt.plot(x,y,'-',linewidth=0.8,color=color,label=label)
         plt.draw()
 
     @staticmethod
-    def plot_final_path(path, map:_map, color='green', show_car=False):
+    def plot_final_path(path, color='green', show_car=False, label:str=None):
         x,y=[],[]
         v = Vehicle()
         for i in range(len(path)):
             x.append(path[i][0])
             y.append(path[i][1])
-            ploter.plot_path(x,y,color)
+            if i == 0:
+                ploter.plot_path(x,y,color,label)
+            else:
+                ploter.plot_path(x,y,color)
             if show_car:
                 points = v.create_polygon(path[i][0], path[i][1], path[i][2])
                 plt.plot(points[:, 0], points[:, 1], linestyle='--', linewidth = 0.4, color = color)

config/config.yaml

@@ -1,4 +1,4 @@
-# config for hybrid a star
+## config for hybrid a star
   steering_angle_num: 5 # steering angle discrete
   dt: 0.6 # s used for compute trajectory distance while expanding nodes
   Benchmark_path: BenchmarkCases # case folder name
@@ -7,20 +7,30 @@
   flag_radius: 18 # m (in this circle area, we use rs curve to connect goal pose)
   extended_num: 1 # extend point at the end of orignal path
 
-  # collision check
+## collision check
   safe_side_dis: 0.1 # m
   safe_fr_dis: 0.1 # m
-  collision_check: distance # circle or distance, choose a method for collision check
+  collision_check: distance # choose a method for collision check: 'circle', 'distance', 
   draw_collision: False # draw collision position while searching new nodes
 
-  # m expand distance for path optimization
-  expand_dis: 0.8
+## path optimization
+  # expand distance for path optimization
+  expand_dis: 0.8 #m
   # weight used for path optimization
   smooth_cost: 5
   compact_cost: 3
   offset_cost: 0.8
   slack_cost: 1
 
+## velocity plan
+  # velocity function
+  velocity_func_type: sin_func # only sin_func for teh moment
   # velocity plan points
   velocity_plan_num: 100 # num
 
+
+## save info
+  # save path
+  save_path: ./solution # do not edit
+  # save pictures
+  pic_path: ./pictures

main.py

@@ -2,7 +2,7 @@
 Author: wenqing-hnu
 Date: 2022-10-20
 LastEditors: wenqing-hnu
-LastEditTime: 2022-11-02
+LastEditTime: 2022-11-04
 FilePath: /HybridAstar/main.py
 Description: the main file of the hybrid a star algorithm for parking
 
@@ -12,6 +12,7 @@
 
 from path_planner import path_planner
 from animation.animation import ploter, plt
+from animation.record_solution import DataRecorder
 from map import costmap
 from velocity_planner import velocity_plan
 from interpolation import path_interpolation
@@ -59,16 +60,16 @@
 
     # create velocity planner
     velocity_planner = velocity_plan.velocity_planner(vehicle=ego_vehicle,
-                                                      velocity_func_type='sin_func')
+                                                      velocity_func_type=config['velocity_func_type'])
 
     # create path optimization planner
     ocp_planner = ocp_optimization.ocp_optimization(
         park_map=park_map, vehicle=ego_vehicle, config=config)
 
     # rapare memory to store path
-    plot_opt_path = []  # store the optimization path
-    plot_insert_path = []  # store the interpolation path
-    plot_ocp_path = []  # store ocp path
+    final_opt_path = []  # store the optimization path
+    final_insert_path = []  # store the interpolation path
+    final_ocp_path = []  # store ocp path
 
     # path planning
     optimal_tf = 0
@@ -95,27 +96,37 @@
             path=insert_path)
         optimal_time_info.append([optimal_ti, optimal_dt])
         # add time information
-        for ocp_i in range(len(ocp_traj)):
+        for ocp_i in ocp_traj:
             t += optimal_dt
-            ocp_traj[ocp_i].append(t)
+            ocp_i.append(t)
         optimal_tf += optimal_ti
 
-        plot_opt_path.extend(opti_path)
-        plot_insert_path.extend(insert_path)
-        plot_ocp_path.extend(ocp_traj)
+        final_opt_path.extend(opti_path)
+        final_insert_path.extend(insert_path)
+        final_ocp_path.extend(ocp_traj)
+
+    # save traj into a csv file
+    DataRecorder.record(save_path=config['save_path'],
+                        save_name=case_name, trajectory=final_ocp_path)
 
     # animation
     print('trajectory_time:', optimal_tf)
     ploter.plot_obstacles(map=park_map)
     park_map.visual_cost_map()
-    ploter.plot_final_path(path=original_path, map=park_map,
+    ploter.plot_final_path(path=original_path, label='Hybrid A*',
                            color='green', show_car=False)
-    ploter.plot_final_path(path=plot_opt_path, map=park_map,
+    ploter.plot_final_path(path=final_opt_path, label='Optimized Path',
                            color='blue', show_car=False)
-    ploter.plot_final_path(path=plot_insert_path, map=park_map,
+    ploter.plot_final_path(path=final_insert_path, label='Interpolation Traj',
                            color='red', show_car=False)
-    ploter.plot_final_path(path=plot_ocp_path, map=park_map,
+    ploter.plot_final_path(path=final_ocp_path, label='Optimized Traj',
                            color='gray', show_car=True)
-
+    plt.legend()
+    fig_name = args.case_name + '.png'
+    fig_path = config['pic_path']
+    if not os.path.exists(fig_path):
+        os.makedirs(fig_path)
+    save_fig = os.path.join(fig_path, fig_name)
+    plt.savefig(save_fig, dpi=600)
     plt.show()
     print('solved')