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

imporve logic of A*

Browse Source
This commit is contained in:
Atsushi Sakai
2018-06-28 22:02:27 +09:00
parent 9ef242575f
commit af062f8fde
1 changed files with 23 additions and 12 deletions
Download Patch File Download Diff File Expand all files Collapse all files

35
PathPlanning/AStar/a_star.py
View File

@@ -1,7 +1,13 @@
"""
A* grid based planning
author: Atsushi Sakai(@Atsushi_twi)
Nikos Kanargias (nkana@tee.gr)
See Wikipedia article (https://en.wikipedia.org/wiki/A*_search_algorithm)
See also code of Christian Careaga (http://code.activestate.com/recipes/578919-python-a-pathfinding-with-binary-heap/)
"""
import matplotlib.pyplot as plt
@@ -59,9 +65,8 @@ def a_star_planning(sx, sy, gx, gy, ox, oy, reso, rr):
while 1:
c_id = min(
openset, key=lambda o: openset[o].cost + calc_h(ngoal, openset[o].x, openset[o].y))
openset, key=lambda o: openset[o].cost + calc_heuristic(ngoal, openset[o]))
current = openset[c_id]
# print("current", current)
# show graph
if show_animation:
@@ -82,7 +87,8 @@ def a_star_planning(sx, sy, gx, gy, ox, oy, reso, rr):
# expand search grid based on motion model
for i in range(len(motion)):
node = Node(current.x + motion[i][0], current.y + motion[i][1],
node = Node(current.x + motion[i][0],
current.y + motion[i][1],
current.cost + motion[i][2], c_id)
n_id = calc_index(node, xw, minx, miny)
@@ -91,22 +97,27 @@ def a_star_planning(sx, sy, gx, gy, ox, oy, reso, rr):
if n_id in closedset:
continue
# Otherwise if it is already in the open set
if n_id in openset:
if openset[n_id].cost > node.cost:
openset[n_id].cost = node.cost
openset[n_id].pind = c_id
else:
if n_id not in openset:
openset[n_id] = node
# 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
rx, ry = calc_fianl_path(ngoal, closedset, reso)
return rx, ry
def calc_h(ngoal, x, y):
w = 10.0 # weight of heuristic
d = w * math.sqrt((ngoal.x - x)**2 + (ngoal.y - y)**2)
def calc_heuristic(n1, n2):
w = 1.0 # weight of heuristic
d = w * math.sqrt((n1.x - n2.x)**2 + (n1.y - n2.y)**2)
return d