optimized tests for speed up

Mapping/rectangle_fitting/rectangle_fitting.py

@@ -54,7 +54,7 @@ class LShapeFitting():
         c1_min = min(c1)
         c2_min = min(c2)
 
-        alpha = - (c1_max - c1_min) * (c2_max - c2_min)
+        alpha = -(c1_max - c1_min) * (c2_max - c2_min)
 
         return alpha
 

PathPlanning/BatchInformedRRTStar/batch_informed_rrtstar.py

@@ -314,7 +314,7 @@ class BITStar(object):
             try:
                 currId = self.nodes[currId]
             except(KeyError):
-                print("Path key error")
+                print("cannot find Path")
                 return []
 
         plan.append(self.start)
@@ -569,7 +569,7 @@ class BITStar(object):
         plt.plot(px, py, "--c")
 
 
-def main():
+def main(maxIter=80):
     print("Starting Batch Informed Trees Star planning")
     obstacleList = [
         (5, 5, 0.5),
@@ -581,7 +581,7 @@ def main():
     ]
 
     bitStar = BITStar(start=[-1, 0], goal=[3, 8], obstacleList=obstacleList,
-                      randArea=[-2, 15])
+                      randArea=[-2, 15], maxIter=maxIter)
     path = bitStar.plan(animation=show_animation)
     print("Done")
 

PathPlanning/DynamicWindowApproach/dynamic_window_approach.py

@@ -165,12 +165,12 @@ def plot_arrow(x, y, yaw, length=0.5, width=0.1):  # pragma: no cover
     plt.plot(x, y)
 
 
-def main():
+def main(gx=10, gy=10):
     print(__file__ + " start!!")
     # initial state [x(m), y(m), yaw(rad), v(m/s), omega(rad/s)]
     x = np.array([0.0, 0.0, math.pi / 8.0, 0.0, 0.0])
     # goal position [x(m), y(m)]
-    goal = np.array([10, 10])
+    goal = np.array([gx, gy])
     # obstacles [x(m) y(m), ....]
     ob = np.array([[-1, -1],
                    [0, 2],

PathPlanning/FrenetOptimalTrajectory/frenet_optimal_trajectory.py

@@ -18,6 +18,8 @@ import copy
 import math
 import cubic_spline_planner
 
+SIM_LOOP = 500
+
 # Parameter
 MAX_SPEED = 50.0 / 3.6  # maximum speed [m/s]
 MAX_ACCEL = 2.0  # maximum acceleration [m/ss]
@@ -329,7 +331,7 @@ def main():
 
     area = 20.0  # animation area length [m]
 
-    for i in range(500):
+    for i in range(SIM_LOOP):
         path = frenet_optimal_planning(
             csp, s0, c_speed, c_d, c_d_d, c_d_dd, ob)
 

PathPlanning/RRT/simple_rrt.py

@@ -144,7 +144,7 @@ class Node():
         self.parent = None
 
 
-def main():
+def main(gx=5.0, gy=10.0):
     print("start simple RRT path planning")
 
     # ====Search Path with RRT====
@@ -157,7 +157,7 @@ def main():
         (9, 5, 2)
     ]  # [x,y,size]
     # Set Initial parameters
-    rrt = RRT(start=[0, 0], goal=[5, 10],
+    rrt = RRT(start=[0, 0], goal=[gx, gy],
               randArea=[-2, 15], obstacleList=obstacleList)
     path = rrt.Planning(animation=show_animation)
 

tests/test_a_star.py

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

tests/test_batch_informed_rrt_star.py

@@ -1,6 +1,11 @@
 from unittest import TestCase
-
-from PathPlanning.BatchInformedRRTStar import batch_informed_rrtstar as m
+import sys
+import os
+sys.path.append(os.path.dirname(__file__) + "/../")
+try:
+    from PathPlanning.BatchInformedRRTStar import batch_informed_rrtstar as m
+except:
+    raise
 
 print(__file__)
 
@@ -9,4 +14,9 @@ class Test(TestCase):
 
     def test1(self):
         m.show_animation = False
-        m.main()
+        m.main(maxIter=10)
+
+
+if __name__ == '__main__':
+    test = Test()
+    test.test1()

tests/test_dynamic_window_approach.py

@@ -1,6 +1,12 @@
 from unittest import TestCase
 
-from PathPlanning.DynamicWindowApproach import dynamic_window_approach as m
+import sys
+import os
+sys.path.append(os.path.dirname(__file__) + "/../")
+try:
+    from PathPlanning.DynamicWindowApproach import dynamic_window_approach as m
+except:
+    raise
 
 print(__file__)
 
@@ -9,4 +15,9 @@ class Test(TestCase):
 
     def test1(self):
         m.show_animation = False
-        m.main()
+        m.main(gx=1.0, gy=1.0)
+
+
+if __name__ == '__main__':
+    test = Test()
+    test.test1()

tests/test_fast_slam1.py

@@ -1,6 +1,12 @@
 from unittest import TestCase
+import sys
+import os
+sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../")
+try:
+    from SLAM.FastSLAM1 import fast_slam1 as m
+except:
+    raise
 
-from SLAM.FastSLAM1 import fast_slam1 as m
 
 print(__file__)
 
@@ -9,4 +15,10 @@ class Test(TestCase):
 
     def test1(self):
         m.show_animation = False
+        m.SIM_TIME = 3.0
         m.main()
+
+
+if __name__ == '__main__':
+    test = Test()
+    test.test1()

tests/test_fast_slam2.py

@@ -1,6 +1,12 @@
 from unittest import TestCase
+import sys
+import os
+sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../")
+try:
+    from SLAM.FastSLAM2 import fast_slam2 as m
+except:
+    raise
 
-from SLAM.FastSLAM2 import fast_slam2 as m
 
 print(__file__)
 
@@ -9,4 +15,10 @@ class Test(TestCase):
 
     def test1(self):
         m.show_animation = False
+        m.SIM_TIME = 3.0
         m.main()
+
+
+if __name__ == '__main__':
+    test = Test()
+    test.test1()

tests/test_frenet_optimal_trajectory.py

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

tests/test_graph_based_slam.py

@@ -1,6 +1,12 @@
 from unittest import TestCase
+import sys
+import os
+sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../")
+try:
+    from SLAM.GraphBasedSLAM import graph_based_slam as m
+except:
+    raise
 
-from SLAM.GraphBasedSLAM import graph_based_slam as m
 
 print(__file__)
 
@@ -9,4 +15,10 @@ class Test(TestCase):
 
     def test1(self):
         m.show_animation = False
+        m.SIM_TIME = 20.0
         m.main()
+
+
+if __name__ == '__main__':
+    test = Test()
+    test.test1()

tests/test_histogram_filter.py

@@ -1,6 +1,13 @@
 from unittest import TestCase
 
-from Localization.histogram_filter import histogram_filter as m
+import sys
+import os
+sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../")
+try:
+    from Localization.histogram_filter import histogram_filter as m
+except:
+    raise
+
 
 print(__file__)
 
@@ -9,4 +16,10 @@ class Test(TestCase):
 
     def test1(self):
         m.show_animation = False
+        m.SIM_TIME = 1.0
         m.main()
+
+
+if __name__ == '__main__':
+    test = Test()
+    test.test1()

tests/test_rrt.py

@@ -1,7 +1,14 @@
 from unittest import TestCase
 
-from PathPlanning.RRT import simple_rrt as m
-from PathPlanning.RRT import rrt_with_pathsmoothing as m1
+import sys
+import os
+sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../")
+try:
+    from PathPlanning.RRT import simple_rrt as m
+    from PathPlanning.RRT import rrt_with_pathsmoothing as m1
+except:
+    raise
+
 
 print(__file__)
 
@@ -10,8 +17,14 @@ class Test(TestCase):
 
     def test1(self):
         m.show_animation = False
-        m.main()
+        m.main(gx=1.0, gy=1.0)
 
     def test2(self):
         m1.show_animation = False
         m1.main()
+
+
+if __name__ == '__main__':
+    test = Test()
+    test.test1()
+    test.test2()