github/PythonRobotics
1
1
Fork 0
mirror of https://github.com/AtsushiSakai/PythonRobotics.git synced 2026-01-14 00:27:55 -05:00
Code Issues Packages Projects Releases Wiki Activity

Merge pull request #194 from AfroDisco/patch-2

Browse Source 
Code live lock when no path is found
This commit is contained in:
Atsushi Sakai
2019-05-30 21:02:23 +09:00
committed by GitHub
parent 6ee01460f2 268b3e8123
commit 7927c86128
1 changed files with 15 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files

18
PathPlanning/ProbabilisticRoadMap/probabilistic_road_map.py
View File

@@ -161,12 +161,18 @@ def generate_roadmap(sample_x, sample_y, rr, obkdtree):
def dijkstra_planning(sx, sy, gx, gy, ox, oy, rr, road_map, sample_x, sample_y):
"""
sx: start x position [m]
sy: start y position [m]
gx: goal x position [m]
gx: goal x position [m]
gy: goal y position [m]
ox: x position list of Obstacles [m]
oy: y position list of Obstacles [m]
reso: grid resolution [m]
rr: robot radius[m]
rr: robot radius [m]
road_map: ??? [m]
sample_x: ??? [m]
sample_y: ??? [m]
@return: Two lists of path coordinates ([x1, x2, ...], [y1, y2, ...]), empty list when no path was found
"""
nstart = Node(sx, sy, 0.0, -1)
@@ -175,9 +181,12 @@ def dijkstra_planning(sx, sy, gx, gy, ox, oy, rr, road_map, sample_x, sample_y):
openset, closedset = dict(), dict()
openset[len(road_map) - 2] = nstart
path_found = True
while True:
if not openset:
print("Cannot find path")
path_found = False
break
c_id = min(openset, key=lambda o: openset[o].cost)
@@ -217,6 +226,9 @@ def dijkstra_planning(sx, sy, gx, gy, ox, oy, rr, road_map, sample_x, sample_y):
openset[n_id].pind = c_id
else:
openset[n_id] = node
if path_found is False:
return [], []
# generate final course
rx, ry = [ngoal.x], [ngoal.y]