github/PythonRobotics
1
1
Fork 0
mirror of https://github.com/AtsushiSakai/PythonRobotics.git synced 2026-01-13 20:28:07 -05:00
Code Issues Packages Projects Releases Wiki Activity

Dubins path bug fix (#521)

Browse Source 
* 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
This commit is contained in:
Jonathan Schwartz
2021-07-01 09:46:32 -05:00
committed by GitHub
parent b0df3c7db3
commit 8f3337e78d
1 changed files with 12 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files

22
PathPlanning/DubinsPath/dubins_path_planning.py
View File

@@ -254,11 +254,13 @@ def generate_local_course(total_length, lengths, mode, max_curvature,
ll = 0.0
for (m, l, i) in zip(mode, lengths, range(len(mode))):
if l > 0.0:
d = step_size
for (m, length, i) in zip(mode, lengths, range(len(mode))):
if length == 0:
continue
elif length > 0.0:
dist = step_size
else:
d = -step_size
dist = -step_size
# set origin state
origin_x, origin_y, origin_yaw = \
@@ -266,22 +268,22 @@ def generate_local_course(total_length, lengths, mode, max_curvature,
index -= 1
if i >= 1 and (lengths[i - 1] * lengths[i]) > 0:
pd = - d - ll
pd = - dist - ll
else:
pd = d - ll
pd = dist - ll
while abs(pd) <= abs(l):
while abs(pd) <= abs(length):
index += 1
path_x, path_y, path_yaw, directions = interpolate(
index, pd, m, max_curvature, origin_x, origin_y, origin_yaw,
path_x, path_y, path_yaw, directions)
pd += d
pd += dist
ll = l - pd - d # calc remain length
ll = length - pd - dist # calc remain length
index += 1
path_x, path_y, path_yaw, directions = interpolate(
index, l, m, max_curvature, origin_x, origin_y, origin_yaw,
index, length, m, max_curvature, origin_x, origin_y, origin_yaw,
path_x, path_y, path_yaw, directions)
if len(path_x) <= 1: