From f299826eb9d775130cf7f1b0eecc0728adebafe8 Mon Sep 17 00:00:00 2001 From: Atsushi Sakai Date: Sat, 30 Jun 2018 13:13:08 +0900 Subject: [PATCH] update code --- PathPlanning/AStar/a_star.py | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/PathPlanning/AStar/a_star.py b/PathPlanning/AStar/a_star.py index 1042b7f5..715ec558 100644 --- a/PathPlanning/AStar/a_star.py +++ b/PathPlanning/AStar/a_star.py @@ -100,9 +100,12 @@ 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 - if node.cost >= node.cost: + 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! rx, ry = calc_fianl_path(ngoal, closedset, reso)