From 8f3337e78d0258386d220192caa97763e17a33d6 Mon Sep 17 00:00:00 2001
From: Jonathan Schwartz <jschwar4@andrew.cmu.edu>
Date: Thu, 1 Jul 2021 09:46:32 -0500
Subject: [PATCH] Dubins path bug fix (#521)

* 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
---
 .../DubinsPath/dubins_path_planning.py        | 22 ++++++++++---------
 1 file changed, 12 insertions(+), 10 deletions(-)

diff --git a/PathPlanning/DubinsPath/dubins_path_planning.py b/PathPlanning/DubinsPath/dubins_path_planning.py
index 9c13198a..64d29e50 100644
--- a/PathPlanning/DubinsPath/dubins_path_planning.py
+++ b/PathPlanning/DubinsPath/dubins_path_planning.py
@@ -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: