github/PythonRobotics
1
1
Fork 0
mirror of https://github.com/AtsushiSakai/PythonRobotics.git synced 2026-02-11 00:44:58 -05:00
Code Issues Packages Projects Releases Wiki Activity

Issue #523 fix (Reeds-Shepp planner handling length=0 case) (#524)

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

* Made len == 0 it's own case, also changed 'l' to 'len' to appease travisCI

* More variable renaming to appease CI

* 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

* Added handling of length=0 case in generate_local_course()

* Missed reverting 'mode' back to 'm' in one spot

* Addressing style issues (line length)
This commit is contained in:
Jonathan Schwartz
2021-07-02 07:53:02 -05:00
committed by GitHub
parent 8f3337e78d
commit 51689d62b9
2 changed files with 13 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files

19
PathPlanning/ReedsSheppPath/reeds_shepp_path_planning.py
View File

@@ -285,8 +285,10 @@ def generate_local_course(total_length, lengths, mode, max_curvature, step_size)
ll = 0.0
for (m, l, i) in zip(mode, lengths, range(len(mode))):
if l > 0.0:
for (m, length, i) in zip(mode, lengths, range(len(mode))):
if length == 0.0:
continue
elif length > 0.0:
d = step_size
else:
d = -step_size
@@ -300,17 +302,19 @@ def generate_local_course(total_length, lengths, mode, max_curvature, step_size)
else:
pd = d - ll
while abs(pd) <= abs(l):
while abs(pd) <= abs(length):
ind += 1
px, py, pyaw, directions = interpolate(
ind, pd, m, max_curvature, ox, oy, oyaw, px, py, pyaw, directions)
ind, pd, m, max_curvature, ox, oy, oyaw,
px, py, pyaw, directions)
pd += d
ll = l - pd - d # calc remain length
ll = length - pd - d # calc remain length
ind += 1
px, py, pyaw, directions = interpolate(
ind, l, m, max_curvature, ox, oy, oyaw, px, py, pyaw, directions)
ind, length, m, max_curvature, ox, oy, oyaw,
px, py, pyaw, directions)
# remove unused data
while px[-1] == 0.0:
@@ -390,7 +394,8 @@ def main():
step_size = 0.1
px, py, pyaw, mode, clen = reeds_shepp_path_planning(
start_x, start_y, start_yaw, end_x, end_y, end_yaw, curvature, step_size)
start_x, start_y, start_yaw, end_x, end_y, end_yaw,
curvature, step_size)
if show_animation: # pragma: no cover
plt.cla()