change RS path code to correct one

PathPlanning/ClosedLoopRRTStar/closed_loop_rrt_star_car.py

@@ -5,6 +5,9 @@ author: AtsushiSakai(@Atsushi_twi)
 
 """
 
+import sys
+sys.path.append("../ReedsSheppPath/")
+
 import random
 import math
 import copy
@@ -18,6 +21,7 @@ show_animation = True
 
 
 target_speed = 10.0 / 3.6
+STEP_SIZE = 0.5
 
 
 class RRT():
@@ -217,7 +221,7 @@ class RRT():
 
         px, py, pyaw, mode, clen = reeds_shepp_path_planning.reeds_shepp_path_planning(
             nearestNode.x, nearestNode.y, nearestNode.yaw,
-            rnd.x, rnd.y, rnd.yaw, unicycle_model.curvature_max)
+            rnd.x, rnd.y, rnd.yaw, unicycle_model.curvature_max, STEP_SIZE)
 
         newNode = copy.deepcopy(nearestNode)
         newNode.x = px[-1]
@@ -227,7 +231,7 @@ class RRT():
         newNode.path_x = px
         newNode.path_y = py
         newNode.path_yaw = pyaw
-        newNode.cost += clen
+        newNode.cost += sum(clen)
         newNode.parent = nind
 
         return newNode

PathPlanning/ClosedLoopRRTStar/reeds_shepp_path_planning.py

@@ -1,90 +0,0 @@
-#! /usr/bin/python
-# -*- coding: utf-8 -*-
-"""
-
-Reeds Shepp path planner sample code
-
-author Atsushi Sakai(@Atsushi_twi)
-
-License MIT
-
-"""
-import reeds_shepp
-import math
-
-
-def plot_arrow(x, y, yaw, length=1.0, width=0.5, fc="r", ec="k"):
-    u"""
-    Plot arrow
-    """
-    import matplotlib.pyplot as plt
-
-    if not isinstance(x, float):
-        for (ix, iy, iyaw) in zip(x, y, yaw):
-            plot_arrow(ix, iy, iyaw)
-    else:
-        plt.arrow(x, y, length * math.cos(yaw), length * math.sin(yaw),
-                  fc=fc, ec=ec, head_width=width, head_length=width)
-        plt.plot(x, y)
-
-
-def reeds_shepp_path_planning(start_x, start_y, start_yaw,
-                              end_x, end_y, end_yaw, curvature):
-    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)
-    xs = [q[0] for q in qs]
-    ys = [q[1] for q in qs]
-    yaw = [q[2] for q in qs]
-
-    xs.append(end_x)
-    ys.append(end_y)
-    yaw.append(end_yaw)
-
-    clen = reeds_shepp.path_length(q0, q1, curvature)
-    pathtypeTuple = reeds_shepp.path_type(q0, q1, curvature)
-
-    ptype = ""
-    for t in pathtypeTuple:
-        if t == 1:
-            ptype += "L"
-        elif t == 2:
-            ptype += "S"
-        elif t == 3:
-            ptype += "R"
-
-    return xs, ys, yaw, ptype, clen
-
-
-if __name__ == '__main__':
-    print("Reeds Shepp path planner sample start!!")
-    import matplotlib.pyplot as plt
-
-    start_x = 1.0  # [m]
-    start_y = 1.0  # [m]
-    start_yaw = math.radians(0.0)  # [rad]
-
-    end_x = -0.0  # [m]
-    end_y = -3.0  # [m]
-    end_yaw = math.radians(-45.0)  # [rad]
-
-    curvature = 1.0
-
-    px, py, pyaw, mode, clen = reeds_shepp_path_planning(
-        start_x, start_y, start_yaw, end_x, end_y, end_yaw, curvature)
-
-    plt.plot(px, py, label="final course " + str(mode))
-
-    # plotting
-    plot_arrow(start_x, start_y, start_yaw)
-    plot_arrow(end_x, end_y, end_yaw)
-
-    for (ix, iy, iyaw) in zip(px, py, pyaw):
-        plot_arrow(ix, iy, iyaw, fc="b")
-    #  print(clen)
-
-    plt.legend()
-    plt.grid(True)
-    plt.axis("equal")
-    plt.show()

tests/test_reeds_shepp_path_planning.py

@@ -7,3 +7,4 @@ class Test(TestCase):
     def test1(self):
         m.show_animation = False
         m.main()
+        m.test()