Merge pull request AtsushiSakai#214 from AtsushiSakai/develop

Fix RRT rewire bug and code clean up

Author: AtsushiSakai

PathPlanning/BatchInformedRRTStar/batch_informed_rrtstar.py

@@ -6,8 +6,9 @@
 
 """
 import math
-import numpy as np
+
 import matplotlib.pyplot as plt
+import numpy as np
 
 show_animation = True
 
@@ -136,8 +137,8 @@ def LRL(alpha, beta, d):
     return t, p, q, mode
 
 
-def dubins_path_planning_from_origin(ex, ey, eyaw, c):
-    # nomalize
+def dubins_path_planning_from_origin(ex, ey, eyaw, c, D_ANGLE):
+    # normalize
     dx = ex
     dy = ey
     D = math.sqrt(dx ** 2.0 + dy ** 2.0)
@@ -165,12 +166,12 @@ def dubins_path_planning_from_origin(ex, ey, eyaw, c):
             bcost = cost
 
     #  print(bmode)
-    px, py, pyaw = generate_course([bt, bp, bq], bmode, c)
+    px, py, pyaw = generate_course([bt, bp, bq], bmode, c, D_ANGLE)
 
     return px, py, pyaw, bmode, bcost
 
 
-def dubins_path_planning(sx, sy, syaw, ex, ey, eyaw, c):
+def dubins_path_planning(sx, sy, syaw, ex, ey, eyaw, c, D_ANGLE=np.deg2rad(10.0)):
     """
     Dubins path plannner
 
@@ -199,7 +200,7 @@ def dubins_path_planning(sx, sy, syaw, ex, ey, eyaw, c):
     leyaw = eyaw - syaw
 
     lpx, lpy, lpyaw, mode, clen = dubins_path_planning_from_origin(
-        lex, ley, leyaw, c)
+        lex, ley, leyaw, c, D_ANGLE)
 
     px = [math.cos(-syaw) * x + math.sin(-syaw)
           * y + sx for x, y in zip(lpx, lpy)]
@@ -210,7 +211,7 @@ def dubins_path_planning(sx, sy, syaw, ex, ey, eyaw, c):
     return px, py, pyaw, mode, clen
 
 
-def generate_course(length, mode, c):
+def generate_course(length, mode, c, D_ANGLE):
 
     px = [0.0]
     py = [0.0]
@@ -221,7 +222,7 @@ def generate_course(length, mode, c):
         if m == "S":
             d = 1.0 * c
         else:  # turning couse
-            d = np.deg2rad(3.0)
+            d = D_ANGLE
 
         while pd < abs(l - d):
             #  print(pd, l)

PathPlanning/ClosedLoopRRTStar/closed_loop_rrt_star_car.py

@@ -259,7 +259,7 @@ def planning(ox, oy, reso,
     return rx, ry
 
 
-def planning_animation(ox, oy, reso):
+def planning_animation(ox, oy, reso):  # pragma: no cover
     px, py = planning(ox, oy, reso)
 
     # animation
@@ -281,7 +281,7 @@ def planning_animation(ox, oy, reso):
     plt.pause(0.1)
 
 
-def main():
+def main():  # pragma: no cover
     print("start!!")
 
     ox = [0.0, 20.0, 50.0, 100.0, 130.0, 40.0, 0.0]