optimize test

PathPlanning/ClosedLoopRRTStar/closed_loop_rrt_star_car.py

@@ -13,7 +13,9 @@ import pure_pursuit
 import matplotlib.pyplot as plt
 
 import sys
-sys.path.append("../ReedsSheppPath/")
+import os
+sys.path.append(os.path.dirname(
+    os.path.abspath(__file__)) + "/../ReedsSheppPath/")
 
 try:
     import reeds_shepp_path_planning
@@ -405,8 +407,8 @@ class Node():
         self.parent = None
 
 
-def main():
-    print("Start rrt start planning")
+def main(gx=6.0, gy=7.0, gyaw=np.deg2rad(90.0), maxIter=500):
+    print("Start" + __file__)
     # ====Search Path with RRT====
     obstacleList = [
         (5, 5, 1),
@@ -422,9 +424,10 @@ def main():
 
     # Set Initial parameters
     start = [0.0, 0.0, np.deg2rad(0.0)]
-    goal = [6.0, 7.0, np.deg2rad(90.0)]
+    goal = [gx, gy, gyaw]
 
-    rrt = RRT(start, goal, randArea=[-2.0, 20.0], obstacleList=obstacleList)
+    rrt = RRT(start, goal, randArea=[-2.0, 20.0],
+              obstacleList=obstacleList, maxIter=maxIter)
     flag, x, y, yaw, v, t, a, d = rrt.Planning(animation=show_animation)
 
     if not flag:

PathPlanning/LQRRRTStar/lqr_rrt_star.py

@@ -12,7 +12,8 @@ import copy
 import math
 import random
 import sys
-sys.path.append("../LQRPlanner/")
+import os
+sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../LQRPlanner/")
 
 try:
     import LQRplanner
@@ -203,8 +204,8 @@ class RRT():
     def find_near_nodes(self, newNode):
         nnode = len(self.nodeList)
         r = 50.0 * math.sqrt((math.log(nnode) / nnode))
-        dlist = [(node.x - newNode.x) ** 2 +
-                 (node.y - newNode.y) ** 2
+        dlist = [(node.x - newNode.x) ** 2
+                 + (node.y - newNode.y) ** 2
                  for node in self.nodeList]
         nearinds = [dlist.index(i) for i in dlist if i <= r ** 2]
         return nearinds
@@ -246,8 +247,8 @@ class RRT():
         plt.pause(0.01)
 
     def get_nearest_index(self, nodeList, rnd):
-        dlist = [(node.x - rnd.x) ** 2 +
-                 (node.y - rnd.y) ** 2
+        dlist = [(node.x - rnd.x) ** 2
+                 + (node.y - rnd.y) ** 2
                  for node in nodeList]
         minind = dlist.index(min(dlist))
 
@@ -283,8 +284,8 @@ class Node():
         self.parent = None
 
 
-def main():
-    print("Start rrt start planning")
+def main(maxIter=200):
+    print("Start " + __file__)
 
     # ====Search Path with RRT====
     obstacleList = [
@@ -300,7 +301,9 @@ def main():
     start = [0.0, 0.0]
     goal = [6.0, 7.0]
 
-    rrt = RRT(start, goal, randArea=[-2.0, 15.0], obstacleList=obstacleList)
+    rrt = RRT(start, goal, randArea=[-2.0, 15.0],
+              obstacleList=obstacleList,
+              maxIter=maxIter)
     path = rrt.planning(animation=show_animation)
 
     # Draw final path

PathPlanning/RRT/simple_rrt.py

@@ -145,7 +145,7 @@ class Node():
 
 
 def main(gx=5.0, gy=10.0):
-    print("start simple RRT path planning")
+    print("start " + __file__)
 
     # ====Search Path with RRT====
     obstacleList = [

PathPlanning/RRTDubins/rrt_dubins.py

@@ -10,7 +10,10 @@ import copy
 import math
 import random
 import sys
-sys.path.append("../DubinsPath/")
+import os
+sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
+                "/../DubinsPath/")
+
 try:
     import dubins_path_planning
 except:
@@ -198,7 +201,7 @@ class Node():
 
 
 def main():
-    print("Start rrt planning")
+    print("Start " + __file__)
     # ====Search Path with RRT====
     obstacleList = [
         (5, 5, 1),

PathPlanning/RRTStarReedsShepp/rrt_star_reeds_shepp.py

@@ -11,7 +11,9 @@ import copy
 import math
 import random
 import sys
-sys.path.append("../ReedsSheppPath/")
+import os
+sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
+                "/../PathPlanning/ReedsSheppPath/")
 
 try:
     import reeds_shepp_path_planning
@@ -196,9 +198,9 @@ class RRT():
         nnode = len(self.nodeList)
         r = 50.0 * math.sqrt((math.log(nnode) / nnode))
         #  r = self.expandDis * 5.0
-        dlist = [(node.x - newNode.x) ** 2
-                 + (node.y - newNode.y) ** 2
-                 + (node.yaw - newNode.yaw) ** 2
+        dlist = [(node.x - newNode.x) ** 2 +
+                 (node.y - newNode.y) ** 2 +
+                 (node.yaw - newNode.yaw) ** 2
                  for node in self.nodeList]
         nearinds = [dlist.index(i) for i in dlist if i <= r ** 2]
         return nearinds
@@ -249,9 +251,9 @@ class RRT():
         #  input()
 
     def GetNearestListIndex(self, nodeList, rnd):
-        dlist = [(node.x - rnd.x) ** 2
-                 + (node.y - rnd.y) ** 2
-                 + (node.yaw - rnd.yaw) ** 2 for node in nodeList]
+        dlist = [(node.x - rnd.x) ** 2 +
+                 (node.y - rnd.y) ** 2 +
+                 (node.yaw - rnd.yaw) ** 2 for node in nodeList]
         minind = dlist.index(min(dlist))
 
         return minind
@@ -285,18 +287,10 @@ class Node():
         self.parent = None
 
 
-def main():
-    print("Start rrt start planning")
+def main(maxIter=200):
+    print("Start " + __file__)
 
     # ====Search Path with RRT====
-    #  obstacleList = [
-    #  (5, 5, 1),
-    #  (3, 6, 2),
-    #  (3, 8, 2),
-    #  (3, 10, 2),
-    #  (7, 5, 2),
-    #  (9, 5, 2)
-    #  ]  # [x,y,size(radius)]
     obstacleList = [
         (5, 5, 1),
         (4, 6, 1),
@@ -313,7 +307,9 @@ def main():
     start = [0.0, 0.0, np.deg2rad(0.0)]
     goal = [6.0, 7.0, np.deg2rad(90.0)]
 
-    rrt = RRT(start, goal, randArea=[-2.0, 15.0], obstacleList=obstacleList)
+    rrt = RRT(start, goal, randArea=[-2.0, 15.0],
+              obstacleList=obstacleList,
+              maxIter=maxIter)
     path = rrt.Planning(animation=show_animation)
 
     # Draw final path

PathPlanning/RRTstar/rrt_star.py

@@ -243,7 +243,7 @@ class Node():
 
 
 def main():
-    print("Start rrt planning")
+    print("Start " + __file__)
 
     # ====Search Path with RRT====
     obstacleList = [

PathPlanning/StateLatticePlanner/state_lattice_planner.py

@@ -6,17 +6,24 @@ author: Atsushi Sakai (@Atsushi_twi)
 
 """
 import sys
-
-sys.path.append("../ModelPredictiveTrajectoryGenerator")
-
+import os
 from matplotlib import pyplot as plt
 import numpy as np
 import math
 import pandas as pd
-import model_predictive_trajectory_generator as planner
-import motion_model
 
-table_path = "./lookuptable.csv"
+sys.path.append(os.path.dirname(os.path.abspath(__file__))
+                + "/../ModelPredictiveTrajectoryGenerator/")
+
+
+try:
+    import model_predictive_trajectory_generator as planner
+    import motion_model
+except:
+    raise
+
+
+table_path = os.path.dirname(os.path.abspath(__file__)) + "/lookuptable.csv"
 
 show_animation = True
 

tests/test_closed_loop_rrt_star_car.py

@@ -1,6 +1,14 @@
 from unittest import TestCase
+import sys
+import os
+sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../")
+sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
+                "/../PathPlanning/ClosedLoopRRTStar/")
+try:
+    from PathPlanning.ClosedLoopRRTStar import closed_loop_rrt_star_car as m
+except:
+    raise
 
-from SLAM.iterative_closest_point import iterative_closest_point as m
 
 print(__file__)
 
@@ -9,4 +17,9 @@ class Test(TestCase):
 
     def test1(self):
         m.show_animation = False
-        m.main()
+        m.main(gx=1.0, gy=0.0, gyaw=0.0, maxIter=5)
+
+
+if __name__ == '__main__':
+    test = Test()
+    test.test1()

tests/test_informed_rrt_star.py

@@ -1,6 +1,13 @@
 from unittest import TestCase
-
-from PathPlanning.InformedRRTStar import informed_rrt_star as m
+import sys
+import os
+sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../")
+sys.path.append(os.path.dirname(os.path.abspath(__file__))
+                + "/../PathPlanning/InformedRRTStar/")
+try:
+    from PathPlanning.InformedRRTStar import informed_rrt_star as m
+except:
+    raise
 
 print(__file__)
 
@@ -10,3 +17,8 @@ class Test(TestCase):
     def test1(self):
         m.show_animation = False
         m.main()
+
+
+if __name__ == '__main__':
+    test = Test()
+    test.test1()

tests/test_lqr_rrt_star.py

@@ -1,9 +1,13 @@
 from unittest import TestCase
-
 import sys
-sys.path.append("./PathPlanning/LQRRRTStar/")
-
-from PathPlanning.LQRRRTStar import lqr_rrt_star as m
+import os
+sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../")
+sys.path.append(os.path.dirname(os.path.abspath(__file__))
+                + "/../PathPlanning/LQRRRTStar/")
+try:
+    from PathPlanning.LQRRRTStar import lqr_rrt_star as m
+except:
+    raise
 
 print(__file__)
 
@@ -12,4 +16,9 @@ class Test(TestCase):
 
     def test1(self):
         m.show_animation = False
-        m.main()
+        m.main(maxIter=5)
+
+
+if __name__ == '__main__':
+    test = Test()
+    test.test1()

tests/test_rrt_dubins.py

@@ -1,11 +1,13 @@
 from unittest import TestCase
 import sys
-sys.path.append("./PathPlanning/RRTDubins/")
-sys.path.append("./PathPlanning/DubinsPath/")
+import os
+sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
+                "/../PathPlanning/RRTDubins/")
+sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
+                "/../PathPlanning/DubinsPath/")
 
 try:
-    from PathPlanning.RRTDubins import rrt_dubins as m
-    # from RRTDubins import rrt_dubins as m
+    import rrt_dubins as m
 except:
     raise
 
@@ -18,3 +20,8 @@ class Test(TestCase):
     def test1(self):
         m.show_animation = False
         m.main()
+
+
+if __name__ == '__main__':
+    test = Test()
+    test.test1()

tests/test_rrt_star.py

@@ -1,6 +1,14 @@
 from unittest import TestCase
+import sys
+import os
+sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
+                "/../PathPlanning/RRTStar/")
+
+try:
+    import rrt_star as m
+except:
+    raise
 
-from PathPlanning.RRTstar import rrt_star as m
 
 print(__file__)
 
@@ -10,3 +18,8 @@ class Test(TestCase):
     def test1(self):
         m.show_animation = False
         m.main()
+
+
+if __name__ == '__main__':
+    test = Test()
+    test.test1()

tests/test_rrt_star_dubins.py

@@ -1,9 +1,14 @@
 from unittest import TestCase
 
 import sys
-sys.path.append("./PathPlanning/RRTStarDubins/")
+import os
+sys.path.append(os.path.dirname(os.path.abspath(__file__))
+                + "/../PathPlanning/RRTStarDubins/")
 
-from PathPlanning.RRTStarDubins import rrt_star_dubins as m
+try:
+    import rrt_star_dubins as m
+except:
+    raise
 
 print(__file__)
 
@@ -13,3 +18,8 @@ class Test(TestCase):
     def test1(self):
         m.show_animation = False
         m.main()
+
+
+if __name__ == '__main__':
+    test = Test()
+    test.test1()

tests/test_rrt_star_reeds_shepp.py

@@ -1,10 +1,17 @@
 from unittest import TestCase
 
 import sys
-sys.path.append("./PathPlanning/RRTStarReedsShepp/")
-sys.path.append("./PathPlanning/ReedsSheppPath/")
+import os
+sys.path.append(os.path.dirname(os.path.abspath(__file__))
+                + "/../PathPlanning/RRTStarReedsShepp/")
+sys.path.append(os.path.dirname(os.path.abspath(__file__))
+                + "/../PathPlanning/ReedsSheppPath/")
+
+try:
+    import rrt_star_reeds_shepp as m
+except:
+    raise
 
-from PathPlanning.RRTStarReedsShepp import rrt_star_reeds_shepp as m
 
 print(__file__)
 
@@ -13,4 +20,9 @@ class Test(TestCase):
 
     def test1(self):
         m.show_animation = False
-        m.main()
+        m.main(maxIter=5)
+
+
+if __name__ == '__main__':
+    test = Test()
+    test.test1()

tests/test_state_lattice_planner.py

@@ -1,12 +1,18 @@
 from unittest import TestCase
 
 import sys
-sys.path.append("./PathPlanning/ModelPredictiveTrajectoryGenerator/")
-sys.path.append("./PathPlanning/StateLatticePlanner/")
 
-from PathPlanning.StateLatticePlanner import state_lattice_planner as m
+import os
+sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
+                "/../PathPlanning/ModelPredictiveTrajectoryGenerator/")
+sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
+                "/../PathPlanning/StateLatticePlanner/")
 
-m.table_path = "./PathPlanning/StateLatticePlanner/lookuptable.csv"
+try:
+    import state_lattice_planner as m
+    import model_predictive_trajectory_generator as m2
+except:
+    raise
 
 print(__file__)
 
@@ -15,4 +21,10 @@ class Test(TestCase):
 
     def test1(self):
         m.show_animation = False
+        m2.show_animation = False
         m.main()
+
+
+if __name__ == '__main__':
+    test = Test()
+    test.test1()