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

Fix the problem that the length of candidate path is not correctly compared. Optimal path may be discarded. (#487)

Browse Source
This commit is contained in:
zarmars
2021-03-16 22:13:11 +08:00
committed by GitHub
parent d391cdbb8c
commit d53ec936bc
1 changed files with 3 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files

5
PathPlanning/ReedsSheppPath/reeds_shepp_path_planning.py
View File

@@ -40,7 +40,8 @@ def plot_arrow(x, y, yaw, length=1.0, width=0.5, fc="r", ec="k"):
def mod2pi(x):
v = np.mod(x, 2.0 * math.pi)
# Be consistent with fmod in cplusplus here.
v = np.mod(x, np.copysign(2.0 * math.pi, x))
if v < -math.pi:
v += 2.0 * math.pi
else:
@@ -76,7 +77,7 @@ def set_path(paths, lengths, ctypes):
for tpath in paths:
typeissame = (tpath.ctypes == path.ctypes)
if typeissame:
if sum(tpath.lengths) - sum(path.lengths) <= 0.01:
if sum(np.abs(tpath.lengths)) - sum(np.abs(path.lengths)) <= 0.01:
return paths # not insert path
path.L = sum([abs(i) for i in lengths])