From 874e0796efb19cb52e2e5d68e923e7c4c4e621b0 Mon Sep 17 00:00:00 2001 From: Leonard Bruns Date: Wed, 1 May 2019 20:28:15 +0200 Subject: [PATCH] Fix bug related to Reeds Shepp lengths --- PathPlanning/ReedsSheppPath/reeds_shepp_path_planning.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/PathPlanning/ReedsSheppPath/reeds_shepp_path_planning.py b/PathPlanning/ReedsSheppPath/reeds_shepp_path_planning.py index da1b4f3494..a607417115 100644 --- a/PathPlanning/ReedsSheppPath/reeds_shepp_path_planning.py +++ b/PathPlanning/ReedsSheppPath/reeds_shepp_path_planning.py @@ -77,13 +77,13 @@ 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.0: return paths # not insert path path.L = sum([abs(i) for i in lengths]) # Base.Test.@test path.L >= 0.01 - if path.L >= 0.01: + if path.L >= 0: paths.append(path) return paths @@ -315,7 +315,7 @@ def generate_local_course(L, lengths, mode, maxc, step_size): ind, l, m, maxc, ox, oy, oyaw, px, py, pyaw, directions) # remove unused data - while px[-1] == 0.0: + while len(px) > 1 and px[-1] == 0.0: px.pop() py.pop() pyaw.pop()