fix behavior when path is not found (#1104)

PathPlanning/HybridAStar/car.py

@@ -97,7 +97,7 @@ def pi_2_pi(angle):
 def move(x, y, yaw, distance, steer, L=WB):
     x += distance * cos(yaw)
     y += distance * sin(yaw)
-    yaw += pi_2_pi(distance * tan(steer) / L)  # distance/2
+    yaw = pi_2_pi(yaw + distance * tan(steer) / L)  # distance/2
 
     return x, y, yaw
 

PathPlanning/HybridAStar/hybrid_a_star.py

@@ -282,7 +282,7 @@ def hybrid_a_star_planning(start, goal, ox, oy, xy_resolution, yaw_resolution):
     while True:
         if not openList:
             print("Error: Cannot find path, No open set")
-            return [], [], []
+            return Path([], [], [], [], 0)
 
         cost, c_id = heapq.heappop(pq)
         if c_id in openList: