Skip to content

Commit 8f3337e

Browse files
author
Jonathan Schwartz
authored
Dubins path bug fix (AtsushiSakai#521)
* Without equals sign, sometimes get points that are in the wrong direction - relative to the points before and after it- when change in x or change in y along path is 0 * Created test script for dubins path generator * Broke == 0 into its own case in dubins planner, also Renaming files to appease CI * Reverting some naming changes * Turns out theres already a test for dubins.. not sure how I missed that * Note to self: run the test cases on your own before throwing them at CI
1 parent b0df3c7 commit 8f3337e

File tree

1 file changed

+12
-10
lines changed

1 file changed

+12
-10
lines changed

PathPlanning/DubinsPath/dubins_path_planning.py

Lines changed: 12 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -254,34 +254,36 @@ def generate_local_course(total_length, lengths, mode, max_curvature,
254254

255255
ll = 0.0
256256

257-
for (m, l, i) in zip(mode, lengths, range(len(mode))):
258-
if l > 0.0:
259-
d = step_size
257+
for (m, length, i) in zip(mode, lengths, range(len(mode))):
258+
if length == 0:
259+
continue
260+
elif length > 0.0:
261+
dist = step_size
260262
else:
261-
d = -step_size
263+
dist = -step_size
262264

263265
# set origin state
264266
origin_x, origin_y, origin_yaw = \
265267
path_x[index], path_y[index], path_yaw[index]
266268

267269
index -= 1
268270
if i >= 1 and (lengths[i - 1] * lengths[i]) > 0:
269-
pd = - d - ll
271+
pd = - dist - ll
270272
else:
271-
pd = d - ll
273+
pd = dist - ll
272274

273-
while abs(pd) <= abs(l):
275+
while abs(pd) <= abs(length):
274276
index += 1
275277
path_x, path_y, path_yaw, directions = interpolate(
276278
index, pd, m, max_curvature, origin_x, origin_y, origin_yaw,
277279
path_x, path_y, path_yaw, directions)
278-
pd += d
280+
pd += dist
279281

280-
ll = l - pd - d # calc remain length
282+
ll = length - pd - dist # calc remain length
281283

282284
index += 1
283285
path_x, path_y, path_yaw, directions = interpolate(
284-
index, l, m, max_curvature, origin_x, origin_y, origin_yaw,
286+
index, length, m, max_curvature, origin_x, origin_y, origin_yaw,
285287
path_x, path_y, path_yaw, directions)
286288

287289
if len(path_x) <= 1:

0 commit comments

Comments
 (0)