diff --git a/Mapping/rectangle_fitting/rectangle_fitting.py b/Mapping/rectangle_fitting/rectangle_fitting.py index 3f650e49..e40fc2e4 100644 --- a/Mapping/rectangle_fitting/rectangle_fitting.py +++ b/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 diff --git a/PathPlanning/BatchInformedRRTStar/batch_informed_rrtstar.py b/PathPlanning/BatchInformedRRTStar/batch_informed_rrtstar.py index f8246738..e8f225d7 100644 --- a/PathPlanning/BatchInformedRRTStar/batch_informed_rrtstar.py +++ b/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") diff --git a/PathPlanning/DynamicWindowApproach/dynamic_window_approach.py b/PathPlanning/DynamicWindowApproach/dynamic_window_approach.py index 40db9a96..3144d9a2 100644 --- a/PathPlanning/DynamicWindowApproach/dynamic_window_approach.py +++ b/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], diff --git a/PathPlanning/FrenetOptimalTrajectory/frenet_optimal_trajectory.py b/PathPlanning/FrenetOptimalTrajectory/frenet_optimal_trajectory.py index e0a338f2..5c756a9d 100644 --- a/PathPlanning/FrenetOptimalTrajectory/frenet_optimal_trajectory.py +++ b/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) diff --git a/PathPlanning/RRT/simple_rrt.py b/PathPlanning/RRT/simple_rrt.py index 4e33524d..c3bbd15f 100644 --- a/PathPlanning/RRT/simple_rrt.py +++ b/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) diff --git a/SLAM/__init__.py b/SLAM/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/tests/test_a_star.py b/tests/test_a_star.py index 1785393d..c1848956 100644 --- a/tests/test_a_star.py +++ b/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() diff --git a/tests/test_batch_informed_rrt_star.py b/tests/test_batch_informed_rrt_star.py index cb245c54..12257285 100644 --- a/tests/test_batch_informed_rrt_star.py +++ b/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() diff --git a/tests/test_dynamic_window_approach.py b/tests/test_dynamic_window_approach.py index 3abadbf5..901bdcce 100644 --- a/tests/test_dynamic_window_approach.py +++ b/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() diff --git a/tests/test_fast_slam1.py b/tests/test_fast_slam1.py index 477b795d..9d9bf895 100644 --- a/tests/test_fast_slam1.py +++ b/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() diff --git a/tests/test_fast_slam2.py b/tests/test_fast_slam2.py index 48c2ba99..2f019089 100644 --- a/tests/test_fast_slam2.py +++ b/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() diff --git a/tests/test_frenet_optimal_trajectory.py b/tests/test_frenet_optimal_trajectory.py index 4e154809..3ba0890b 100644 --- a/tests/test_frenet_optimal_trajectory.py +++ b/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() diff --git a/tests/test_graph_based_slam.py b/tests/test_graph_based_slam.py index 13524366..7d285de6 100644 --- a/tests/test_graph_based_slam.py +++ b/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() diff --git a/tests/test_histogram_filter.py b/tests/test_histogram_filter.py index f39995b2..6d7df824 100644 --- a/tests/test_histogram_filter.py +++ b/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() diff --git a/tests/test_rrt.py b/tests/test_rrt.py index 5402ade5..25aa5810 100644 --- a/tests/test_rrt.py +++ b/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()