From ddb976aa48afadf8e92f08e02f04ef99f7bc0ccd Mon Sep 17 00:00:00 2001
From: Atsushi Sakai <asakai.amsl+github@gmail.com>
Date: Fri, 6 Apr 2018 16:14:54 +0900
Subject: [PATCH] keep coding

---
 PathPlanning/HybridAStar/hybrid_a_star.py | 14 +++++++++++---
 1 file changed, 11 insertions(+), 3 deletions(-)

diff --git a/PathPlanning/HybridAStar/hybrid_a_star.py b/PathPlanning/HybridAStar/hybrid_a_star.py
index 13189cf3..29278cd8 100644
--- a/PathPlanning/HybridAStar/hybrid_a_star.py
+++ b/PathPlanning/HybridAStar/hybrid_a_star.py
@@ -10,7 +10,7 @@ import sys
 sys.path.append("../ReedsSheppPath/")
 
 import math
-import numpy as np
+#  import numpy as np
 import scipy.spatial
 import matplotlib.pyplot as plt
 import reeds_shepp_path_planning as rs
@@ -132,6 +132,15 @@ def hybrid_a_star_planning(start, goal, ox, oy, xyreso, yawreso):
     openList[calc_index(nstart, c)] = nstart
     heapq.heappush(pq, (calc_index(nstart, c), calc_cost(nstart, h, ngoal, c)))
 
+    while True:
+        if not openList:
+            print("Error: Cannot find path, No open set")
+            return [], [], []
+
+        c_id, cost = heapq.heappop(pq)
+        current = openList.pop(c_id)
+        #  print(current)
+
     rx, ry, ryaw = [], [], []
 
     return rx, ry, ryaw
@@ -155,8 +164,7 @@ def calc_index(node, c):
 
 
 def main():
-    print("Start rrt start planning")
-    # ====Search Path with RRT====
+    print("Start Hybrid A* planning")
 
     ox, oy = [], []