change RS path code to correct one

AtsushiSakai · AtsushiSakai · commit 0b1ca74408f8 · 2018-02-07T13:46:13.000-08:00
diff --git a/PathPlanning/ClosedLoopRRTStar/closed_loop_rrt_star_car.py b/PathPlanning/ClosedLoopRRTStar/closed_loop_rrt_star_car.py
@@ -5,6 +5,9 @@
 
 """
 
+import sys
+sys.path.append("../ReedsSheppPath/")
+
 import random
 import math
 import copy
@@ -18,6 +21,7 @@
 
 
 target_speed = 10.0 / 3.6
+STEP_SIZE = 0.5
 
 
 class RRT():
@@ -217,7 +221,7 @@ def steer(self, rnd, nind):
 
         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 @@ def steer(self, rnd, nind):
         newNode.path_x = px
         newNode.path_y = py
         newNode.path_yaw = pyaw
-        newNode.cost += clen
+        newNode.cost += sum(clen)
         newNode.parent = nind
 
         return newNode
diff --git a/PathPlanning/ClosedLoopRRTStar/reeds_shepp_path_planning.py b/PathPlanning/ClosedLoopRRTStar/reeds_shepp_path_planning.py
diff --git a/tests/test_reeds_shepp_path_planning.py b/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()
