From 51689d62b9c0c2d245f59fd711ff36774d89a833 Mon Sep 17 00:00:00 2001
From: Jonathan Schwartz <jonbs98@gmail.com>
Date: Fri, 2 Jul 2021 07:53:02 -0500
Subject: [PATCH] Issue #523 fix (Reeds-Shepp planner handling length=0 case)
 (#524)

* 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)
---
 .../DubinsPath/dubins_path_planning.py        |  2 +-
 .../reeds_shepp_path_planning.py              | 19 ++++++++++++-------
 2 files changed, 13 insertions(+), 8 deletions(-)

diff --git a/PathPlanning/DubinsPath/dubins_path_planning.py b/PathPlanning/DubinsPath/dubins_path_planning.py
index 64d29e50..a813d79b 100644
--- a/PathPlanning/DubinsPath/dubins_path_planning.py
+++ b/PathPlanning/DubinsPath/dubins_path_planning.py
@@ -255,7 +255,7 @@ def generate_local_course(total_length, lengths, mode, max_curvature,
     ll = 0.0
 
     for (m, length, i) in zip(mode, lengths, range(len(mode))):
-        if length == 0:
+        if length == 0.0:
             continue
         elif length > 0.0:
             dist = step_size
diff --git a/PathPlanning/ReedsSheppPath/reeds_shepp_path_planning.py b/PathPlanning/ReedsSheppPath/reeds_shepp_path_planning.py
index 7808f2c9..0b31e737 100644
--- a/PathPlanning/ReedsSheppPath/reeds_shepp_path_planning.py
+++ b/PathPlanning/ReedsSheppPath/reeds_shepp_path_planning.py
@@ -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()