AtHeartEngineer/PythonRobotics
1
1
Fork 0
mirror of https://github.com/AtsushiSakai/PythonRobotics.git synced 2026-04-22 03:00:41 -04:00
Code Issues Packages Projects Releases Wiki Activity

improve code

Browse Source
This commit is contained in:
Atsushi Sakai
2018-06-30 12:02:23 +09:00
parent b67d9f6a0e
commit bb4b07247c
1 changed files with 4 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files

17
PathPlanning/AStar/a_star.py
View File

@@ -92,23 +92,14 @@ def a_star_planning(sx, sy, gx, gy, ox, oy, reso, rr):
current.cost + motion[i][2], c_id)
n_id = calc_index(node, xw, minx, miny)
if not verify_node(node, obmap, minx, miny, maxx, maxy):
continue
if n_id in closedset:
continue
if n_id not in openset:
openset[n_id] = node
if not verify_node(node, obmap, minx, miny, maxx, maxy):
continue
# The distance from start to a neighbor.
# The "dist_between" function may vary as per the solution requirements.
if node.cost >= openset[n_id].cost:
continue # This is not a better path.
# This path is the best until now. Record it!
openset[n_id].cost = node.cost
openset[n_id].pind = c_id
if node.cost < node.cost or n_id not in openset:
openset[n_id] = node # update or insert new node
rx, ry = calc_fianl_path(ngoal, closedset, reso)