Skip to content

Commit 268b3e8

Browse files
authored
Code live lock when no path is found
Added a simple boolean variable to track the state of the algorithm and return something empty when no path is found. Added and fixed (partially) the documentation
1 parent 6ee0146 commit 268b3e8

File tree

1 file changed

+15
-3
lines changed

1 file changed

+15
-3
lines changed

PathPlanning/ProbabilisticRoadMap/probabilistic_road_map.py

Lines changed: 15 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -161,12 +161,18 @@ def generate_roadmap(sample_x, sample_y, rr, obkdtree):
161161

162162
def dijkstra_planning(sx, sy, gx, gy, ox, oy, rr, road_map, sample_x, sample_y):
163163
"""
164+
sx: start x position [m]
165+
sy: start y position [m]
164166
gx: goal x position [m]
165-
gx: goal x position [m]
167+
gy: goal y position [m]
166168
ox: x position list of Obstacles [m]
167169
oy: y position list of Obstacles [m]
168-
reso: grid resolution [m]
169-
rr: robot radius[m]
170+
rr: robot radius [m]
171+
road_map: ??? [m]
172+
sample_x: ??? [m]
173+
sample_y: ??? [m]
174+
175+
@return: Two lists of path coordinates ([x1, x2, ...], [y1, y2, ...]), empty list when no path was found
170176
"""
171177

172178
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):
175181
openset, closedset = dict(), dict()
176182
openset[len(road_map) - 2] = nstart
177183

184+
path_found = True
185+
178186
while True:
179187
if not openset:
180188
print("Cannot find path")
189+
path_found = False
181190
break
182191

183192
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):
217226
openset[n_id].pind = c_id
218227
else:
219228
openset[n_id] = node
229+
230+
if path_found is False:
231+
return [], []
220232

221233
# generate final course
222234
rx, ry = [ngoal.x], [ngoal.y]

0 commit comments

Comments
 (0)