change class map name and fix theta continuity bug

animation/animation.py

@@ -2,7 +2,7 @@
 Author: wenqing-hnu
 Date: 2022-10-20
 LastEditors: wenqing-hnu
-LastEditTime: 2022-11-06
+LastEditTime: 2022-11-08
 FilePath: /Automated Valet Parking/animation/animation.py
 Description: animation for the solving process
 
@@ -14,12 +14,12 @@
 import numpy as np
 import matplotlib.pyplot as plt
 import matplotlib.animation as animation
-from map.costmap import Vehicle, _map
+from map.costmap import Vehicle, Map
 
 class ploter:
 
     @staticmethod
-    def plot_obstacles(map:_map, fig_id=None):
+    def plot_obstacles(map:Map, fig_id=None):
         plt.ion()
         if fig_id == None:
             fig_id = 1

collision_check/collision_check.py

@@ -2,7 +2,7 @@
 Author: wenqing-hnu
 Date: 2022-10-20
 LastEditors: wenqing-hnu
-LastEditTime: 2022-11-06
+LastEditTime: 2022-11-08
 FilePath: /Automated Valet Parking/collision_check/collision_check.py
 Description: collision check
 
@@ -14,12 +14,12 @@
 from typing import Tuple
 import numpy as np
 import matplotlib.pyplot as plt
-from map.costmap import _map, Vehicle
+from map.costmap import Map, Vehicle
 
 
 class collision_checker:
     def __init__(self,
-                 map: _map,
+                 map: Map,
                  vehicle: Vehicle = None,
                  config: dict = None) -> None:
         self.map = map
@@ -82,7 +82,7 @@ class two_circle_checker(collision_checker):
     use two circle to present car body for collision check
     '''
 
-    def __init__(self, map: _map, vehicle: Vehicle = None, config: dict = None) -> None:
+    def __init__(self, map: Map, vehicle: Vehicle = None, config: dict = None) -> None:
         super().__init__(map, vehicle, config)
 
     def check(self, node_x, node_y, theta) -> bool:
@@ -138,7 +138,7 @@ def check(self, node_x, node_y, theta) -> bool:
 
 
 class distance_checker(collision_checker):
-    def __init__(self, map: _map, vehicle: Vehicle = None, config: dict = None) -> None:
+    def __init__(self, map: Map, vehicle: Vehicle = None, config: dict = None) -> None:
         super().__init__(map, vehicle, config)
 
     def check(self, node_x, node_y, theta) -> bool:

interpolation/path_interpolation.py

@@ -2,7 +2,7 @@
 Author: wenqing-hnu
 Date: 2022-10-20
 LastEditors: wenqing-hnu
-LastEditTime: 2022-11-06
+LastEditTime: 2022-11-08
 FilePath: /Automated Valet Parking/interpolation/path_interpolation.py
 Description: interpolation for the optimized path
 
@@ -143,15 +143,27 @@ def cubic_interpolation(self,
             theta_angle = pi_2_pi(theta_angle)
             insert_path[i+1][2] = theta_angle
 
-        # corect the theta angle
+        # check the continuity of the theta angle
+        # for i in range(len(insert_path)-1):
+        #     if abs(insert_path[i+1][2] - insert_path[i][2]) <= np.pi:
+        #         continue
+        #     else:
+        #         while insert_path[i+1][2] - insert_path[i][2] > np.pi:
+        #             insert_path[i+1][2] -= np.pi
+        #         while insert_path[i+1][2] - insert_path[i][2] < np.pi:
+        #             insert_path[i+1][2] += np.pi
+
+        # check the theta continuity
         for i in range(len(insert_path)-1):
-            if abs(insert_path[i+1][2] - insert_path[i][2]) <= np.pi:
+            if abs(insert_path[i][2] - insert_path[i+1][2]) <= math.pi:
                 continue
             else:
-                while insert_path[i+1][2] - insert_path[i][2] > np.pi:
-                    insert_path[i+1][2] -= np.pi
-                while insert_path[i+1][2] - insert_path[i][2] < np.pi:
-                    insert_path[i+1][2] += np.pi
+                if (insert_path[i+1][2] - insert_path[i][2]) < 0:
+                    while (insert_path[i+1][2] - insert_path[i][2]) < -math.pi:
+                        insert_path[i+1][2] += 2*math.pi
+                else:
+                    while (insert_path[i+1][2] - insert_path[i][2]) > math.pi:
+                        insert_path[i+1][2] -= 2*math.pi
 
         # compute steering angle and check the theta
         for i in range(len(insert_path) - 1):

main.py

@@ -2,7 +2,7 @@
 Author: wenqing-hnu
 Date: 2022-10-20
 LastEditors: wenqing-hnu
-LastEditTime: 2022-11-06
+LastEditTime: 2022-11-09
 FilePath: /Automated Valet Parking/main.py
 Description: the main file of the hybrid a star algorithm for parking
 
@@ -19,16 +19,14 @@
 from optimization import path_optimazition, ocp_optimization
 from config import read_config
 
-import yaml
 import os
-import copy
 
 import argparse
 
 
 def main(file, config):
     # create the park map
-    park_map = costmap._map(
+    park_map = costmap.Map(
         file=file, discrete_size=config['map_discrete_size'])
 
     # create vehicle
@@ -100,7 +98,7 @@ def main(file, config):
     # animation
     print('trajectory_time:', optimal_tf)
     ploter.plot_obstacles(map=park_map)
-    # park_map.visual_cost_map()
+    park_map.visual_cost_map()
     ploter.plot_final_path(path=original_path, label='Hybrid A*',
                            color='green', show_car=False)
     ploter.plot_final_path(path=final_opt_path, label='Optimized Path',
@@ -127,7 +125,7 @@ def main(file, config):
 if __name__ == '__main__':
     parser = argparse.ArgumentParser(description='hybridAstar')
     parser.add_argument("--config_name", type=str, default="config")
-    parser.add_argument("--case_name", type=str, default="Case1")
+    parser.add_argument("--case_name", type=str, default="Case5")
     args = parser.parse_args()
 
     # initial

map/costmap.py

@@ -2,7 +2,7 @@
 Author: wenqing-hnu
 Date: 2022-10-20
 LastEditors: wenqing-hnu
-LastEditTime: 2022-11-06
+LastEditTime: 2022-11-08
 FilePath: /Automated Valet Parking/map/costmap.py
 Description: generate cost map
 
@@ -156,7 +156,7 @@ def read(file):
         return case
 
 
-class _map:
+class Map:
     def __init__(self,
                  discrete_size: np.float64 = 0.1,
                  file: string = None) -> None:

optimization/ocp_optimization.py

@@ -2,7 +2,7 @@
 Author: wenqing-hnu
 Date: 2022-10-20
 LastEditors: wenqing-hnu
-LastEditTime: 2022-11-06
+LastEditTime: 2022-11-09
 FilePath: /Automated Valet Parking/optimization/ocp_optimization.py
 Description: use ipopt to solve the optimization problem
 
@@ -11,9 +11,9 @@
 
 
 from __future__ import division
-from map.costmap import _map, Vehicle
+from map.costmap import Map, Vehicle
 import math
-import copy
+from path_planner import rs_curve
 import numpy as np
 
 import pyomo.environ as pyo
@@ -25,7 +25,7 @@
 
 class ocp_optimization:
     def __init__(self,
-                 park_map: _map,
+                 park_map: Map,
                  vehicle: Vehicle,
                  config: dict) -> None:
         self.config = config
@@ -46,7 +46,7 @@ def compute_collision_H(self, path):
         '''
 
         # get near obstacles and vehicle
-        def get_near_obstacles(node_x, node_y, theta, map: _map, config):
+        def get_near_obstacles(node_x, node_y, theta, map: Map, config):
             '''
             this function is only used for distance check method
             return the obstacles x and y, vehicle boundary
@@ -129,7 +129,7 @@ def compute_hori_ver_dis(point, k, b, theta):
         Y_min = []
         # create AABB boundary and get the near obstacles position
         for p in original_path:
-            x, y, theta = p[0], p[1], p[2]
+            x, y, theta = p[0], p[1], rs_curve.pi_2_pi(p[2])
             near_obstacles_range, vehicle_boundary = get_near_obstacles(node_x=x, node_y=y,
                                                                         theta=theta, map=self.map, config=self.config)
             # compute k and b
@@ -521,7 +521,7 @@ def bound(model, i):
                 y_bounds = (y_min[k], y_max[k])
                 return y_bounds
             elif (i-2) % 7 == 0:
-                theta_bounds = (-3.1415926, 3.1415926)
+                theta_bounds = (2*-3.1415926, 2*3.1415926)
                 return theta_bounds
             elif (i-3) % 7 == 0:
                 if i == 3:
@@ -630,9 +630,13 @@ def kinematic_constraint(model, i):
             if index % 7 == 0 and index > 0:
                 optimal_traj.append(points)
                 points = []
-            points.append(pyo.value(model.variables[index]))
-            print('solution', index)
-            print(pyo.value(model.variables[index]))
+            if (index-2) % 7 == 0:
+                _theta = rs_curve.pi_2_pi(pyo.value(model.variables[index]))
+                points.append(_theta)
+            else:
+                points.append(pyo.value(model.variables[index]))
+            # print('solution', index)
+            # print(pyo.value(model.variables[index]))
 
         optimal_traj.append(points)
 

optimization/path_optimazition.py

@@ -2,7 +2,7 @@
 Author: wenqing-hnu
 Date: 2022-10-20
 LastEditors: wenqing-hnu
-LastEditTime: 2022-11-06
+LastEditTime: 2022-11-08
 FilePath: /Automated Valet Parking/optimization/path_optimazition.py
 Description: smooth the initial path 
 
@@ -14,13 +14,13 @@
 import numpy as np
 import math
 from cvxopt import matrix, solvers
-from map.costmap import _map, Vehicle
+from map.costmap import Map, Vehicle
 import scipy.spatial as spatial
 
 
 class path_opti:
     def __init__(self,
-                 park_map: _map,
+                 park_map: Map,
                  vehicle: Vehicle,
                  config: dict) -> None:
         self.original_path = None
@@ -232,7 +232,7 @@ def compute_collision_H(self):
 
         # get near obstacles and vehicle
 
-        def get_near_obstacles(node_x, node_y, theta, map: _map, config):
+        def get_near_obstacles(node_x, node_y, theta, map: Map, config):
             '''
             this function is only used for distance check method
             return the obstacles x and y, vehicle boundary

path_planner/hybrid_a_star.py

@@ -2,7 +2,7 @@
 Author: wenqing-hnu
 Date: 2022-10-20
 LastEditors: wenqing-hnu
-LastEditTime: 2022-11-06
+LastEditTime: 2022-11-08
 FilePath: /Automated Valet Parking/path_planner/hybrid_a_star.py
 Description: hybrid a star 
 
@@ -12,7 +12,7 @@
 import numpy as np
 import math
 import queue
-from map.costmap import _map, Vehicle
+from map.costmap import Map, Vehicle
 from collision_check import collision_check
 from path_planner.compute_h import Dijkstra
 from path_planner import rs_curve
@@ -71,7 +71,7 @@ def __lt__(self, other):
 class hybrid_a_star:
     def __init__(self,
                  config: dict,
-                 park_map: _map,
+                 park_map: Map,
                  vehicle: Vehicle) -> None:
 
         # create vehicle

path_planner/path_planner.py

@@ -2,7 +2,7 @@
 Author: wenqing-hnu
 Date: 2022-10-20
 LastEditors: wenqing-hnu
-LastEditTime: 2022-11-06
+LastEditTime: 2022-11-08
 FilePath: /Automated Valet Parking/path_planner/path_planner.py
 Description: path plan
 
@@ -17,15 +17,15 @@
 
 from path_planner.hybrid_a_star import hybrid_a_star
 from animation.animation import ploter
-from map.costmap import Vehicle, _map
+from map.costmap import Vehicle, Map
 from collision_check import collision_check
 from path_planner.rs_curve import PATH
 
 
 class path_planner:
     def __init__(self,
                  config: dict = None,
-                 map: _map = None,
+                 map: Map = None,
                  vehicle: Vehicle = None) -> None:
         self.config = config
         self.map = map

path_planner/rs_curve.py

@@ -2,7 +2,7 @@
 Author: wenqing-hnu
 Date: 2022-10-20
 LastEditors: wenqing-hnu
-LastEditTime: 2022-11-06
+LastEditTime: 2022-11-08
 FilePath: /Automated Valet Parking/path_planner/rs_curve.py
 Description: rs curve for hybrid a star
 
@@ -13,7 +13,7 @@
 import math
 import numpy as np
 import matplotlib.pyplot as plt
-from map.costmap import _map
+
 
 '''
 This file is surrported by this repo:https://github.com/zhm-real/CurvesGenerator