diff --git a/PathPlanning/ClosedLoopRRTStar/closed_loop_rrt_star_car.py b/PathPlanning/ClosedLoopRRTStar/closed_loop_rrt_star_car.py index 86bcf8b3..8af118fa 100644 --- a/PathPlanning/ClosedLoopRRTStar/closed_loop_rrt_star_car.py +++ b/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: diff --git a/PathPlanning/LQRRRTStar/lqr_rrt_star.py b/PathPlanning/LQRRRTStar/lqr_rrt_star.py index c34f0b12..0fb25fe4 100644 --- a/PathPlanning/LQRRRTStar/lqr_rrt_star.py +++ b/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 diff --git a/PathPlanning/RRT/simple_rrt.py b/PathPlanning/RRT/simple_rrt.py index c3bbd15f..3a40c672 100644 --- a/PathPlanning/RRT/simple_rrt.py +++ b/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 = [ diff --git a/PathPlanning/RRTDubins/rrt_dubins.py b/PathPlanning/RRTDubins/rrt_dubins.py index 0c5d41df..54752ef8 100644 --- a/PathPlanning/RRTDubins/rrt_dubins.py +++ b/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), diff --git a/PathPlanning/RRTStarReedsShepp/rrt_star_reeds_shepp.py b/PathPlanning/RRTStarReedsShepp/rrt_star_reeds_shepp.py index af817fc8..6225071e 100644 --- a/PathPlanning/RRTStarReedsShepp/rrt_star_reeds_shepp.py +++ b/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 diff --git a/PathPlanning/RRTstar/rrt_star.py b/PathPlanning/RRTstar/rrt_star.py index b05da7d4..3ca43b7c 100644 --- a/PathPlanning/RRTstar/rrt_star.py +++ b/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 = [ diff --git a/PathPlanning/StateLatticePlanner/state_lattice_planner.py b/PathPlanning/StateLatticePlanner/state_lattice_planner.py index 19018e97..42460906 100644 --- a/PathPlanning/StateLatticePlanner/state_lattice_planner.py +++ b/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 diff --git a/tests/test_closed_loop_rrt_star_car.py b/tests/test_closed_loop_rrt_star_car.py index 9c71688e..d8dd84fe 100644 --- a/tests/test_closed_loop_rrt_star_car.py +++ b/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() diff --git a/tests/test_informed_rrt_star.py b/tests/test_informed_rrt_star.py index a10b3e02..1aab45ce 100644 --- a/tests/test_informed_rrt_star.py +++ b/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() diff --git a/tests/test_lqr_rrt_star.py b/tests/test_lqr_rrt_star.py index 17b363c0..12964e1e 100644 --- a/tests/test_lqr_rrt_star.py +++ b/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() diff --git a/tests/test_rrt_dubins.py b/tests/test_rrt_dubins.py index 659297b5..d55888a4 100644 --- a/tests/test_rrt_dubins.py +++ b/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() diff --git a/tests/test_rrt_star.py b/tests/test_rrt_star.py index 1b6cf6d4..a07bb74a 100644 --- a/tests/test_rrt_star.py +++ b/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() diff --git a/tests/test_rrt_star_dubins.py b/tests/test_rrt_star_dubins.py index eaeb1908..f5983de6 100644 --- a/tests/test_rrt_star_dubins.py +++ b/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() diff --git a/tests/test_rrt_star_reeds_shepp.py b/tests/test_rrt_star_reeds_shepp.py index 2f88338d..c71cd172 100644 --- a/tests/test_rrt_star_reeds_shepp.py +++ b/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() diff --git a/tests/test_state_lattice_planner.py b/tests/test_state_lattice_planner.py index 3907182e..955209ae 100644 --- a/tests/test_state_lattice_planner.py +++ b/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()