Dubins path bug fix (AtsushiSakai#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

Author: Jonathan Schwartz

PathPlanning/DubinsPath/dubins_path_planning.py

@@ -254,34 +254,36 @@ 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 = \
             path_x[index], path_y[index], path_yaw[index]
 
         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: