fix astar logic

AtsushiSakai · AtsushiSakai · commit 26fb451fbf43 · 2018-09-16T09:18:23.000+09:00
diff --git a/PathPlanning/AStar/a_star.py b/PathPlanning/AStar/a_star.py
@@ -99,14 +99,10 @@ def a_star_planning(sx, sy, gx, gy, ox, oy, reso, rr):
 
             if n_id not in openset:
                 openset[n_id] = node  # Discover a new node
-
-            tcost = current.cost + calc_heuristic(current, node)
-
-            if tcost >= node.cost:
-                continue  # this is not a better path
-
-            node.cost = tcost
-            openset[n_id] = node  # This path is the best unitl now. record it!
+            else:
+                if openset[n_id].cost >= node.cost:
+                    # This path is the best until now. record it!
+                    openset[n_id] = node
 
     rx, ry = calc_fianl_path(ngoal, closedset, reso)
 
