fix bug of reedsshepppath

Author: AtsushiSakai

PathPlanning/CRRRTStar/cr_rrt_star_car.py

@@ -349,7 +349,7 @@ def DrawGraph(self, rnd=None):
         for (ox, oy, size) in obstacleList:
             plt.plot(ox, oy, "ok", ms=30 * size)
 
-        reeds_shepp_path_planning.plot_arrow(
+        reeds_shepp_path_planning.plot_arro/Users/atsushisakai/Dropbox/Program/berkeley/RacerCourseOptimization/cvxpy/stanford/mpc_path_planner_common.pyw(
             self.start.x, self.start.y, self.start.yaw)
         reeds_shepp_path_planning.plot_arrow(
             self.end.x, self.end.y, self.end.yaw)

PathPlanning/DubinsPath/dubins_path_planning.py

@@ -132,11 +132,11 @@ def LRL(alpha, beta, d):
     c_ab = math.cos(alpha - beta)
 
     mode = ["L", "R", "L"]
-    tmp_lrl = (6. - d * d + 2 * c_ab + 2 * d * (- sa + sb)) / 8.
+    tmp_lrl = (6.0 - d * d + 2.0 * c_ab + 2.0 * d * (- sa + sb)) / 8.0
     if abs(tmp_lrl) > 1:
         return None, None, None, mode
     p = mod2pi(2 * math.pi - math.acos(tmp_lrl))
-    t = mod2pi(-alpha - math.atan2(ca - cb, d + sa - sb) + p / 2.)
+    t = mod2pi(-alpha - math.atan2(ca - cb, d + sa - sb) + p / 2.0)
     q = mod2pi(mod2pi(beta) - alpha - t + mod2pi(p))
 
     return t, p, q, mode

PathPlanning/RRTStarCar_reeds_sheep/reeds_shepp_path_planning.py

@@ -30,10 +30,10 @@ def plot_arrow(x, y, yaw, length=1.0, width=0.5, fc="r", ec="k"):
 
 def reeds_shepp_path_planning(start_x, start_y, start_yaw,
                               end_x, end_y, end_yaw, curvature):
+    step_size = 0.1
     q0 = [start_x, start_y, start_yaw]
     q1 = [end_x, end_y, end_yaw]
-    step_size = 0.1
-    qs = reeds_shepp.path_sample(q0, q1, curvature, step_size)
+    qs = reeds_shepp.path_sample(q0, q1, 1.0 / curvature, step_size)
     xs = [q[0] for q in qs]
     ys = [q[1] for q in qs]
     yaw = [q[2] for q in qs]
@@ -42,8 +42,8 @@ def reeds_shepp_path_planning(start_x, start_y, start_yaw,
     ys.append(end_y)
     yaw.append(end_yaw)
 
-    clen = reeds_shepp.path_length(q0, q1, curvature)
-    pathtypeTuple = reeds_shepp.path_type(q0, q1, curvature)
+    clen = reeds_shepp.path_length(q0, q1, 1.0 / curvature)
+    pathtypeTuple = reeds_shepp.path_type(q0, q1, 1.0 / curvature)
 
     ptype = ""
     for t in pathtypeTuple:

PathPlanning/RRTStarCar_reeds_sheep/rrt_star_car.py

@@ -22,7 +22,7 @@ class RRT():
     """
 
     def __init__(self, start, goal, obstacleList, randArea,
-                 goalSampleRate=10, maxIter=2000):
+                 goalSampleRate=10, maxIter=1000):
         u"""
         Setting Parameter
 
@@ -48,6 +48,7 @@ def Planning(self, animation=True):
 
         self.nodeList = [self.start]
         for i in range(self.maxIter):
+            print(i)
             rnd = self.get_random_point()
             nind = self.GetNearestListIndex(self.nodeList, rnd)
 
@@ -317,4 +318,4 @@ def __init__(self, x, y, yaw):
 
     plt.show()
 
-    matplotrecorder.save_movie("animation.gif", 0.1)
+    #  matplotrecorder.save_movie("animation.gif", 0.1)