Skip to content

Commit 3146531

Browse files
AtsushiSakaiAtsushiSakai
committed
optimize test
1 parent 0e71384 commit 3146531

15 files changed

+165
-65
lines changed

‎PathPlanning/ClosedLoopRRTStar/closed_loop_rrt_star_car.py

Lines changed: 8 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -13,7 +13,9 @@
1313
import matplotlib.pyplot as plt
1414

1515
import sys
16-
sys.path.append("../ReedsSheppPath/")
16+
import os
17+
sys.path.append(os.path.dirname(
18+
os.path.abspath(__file__)) + "/../ReedsSheppPath/")
1719

1820
try:
1921
import reeds_shepp_path_planning
@@ -405,8 +407,8 @@ def __init__(self, x, y, yaw):
405407
self.parent = None
406408

407409

408-
def main():
409-
print("Start rrt start planning")
410+
def main(gx=6.0, gy=7.0, gyaw=np.deg2rad(90.0), maxIter=500):
411+
print("Start" + __file__)
410412
# ====Search Path with RRT====
411413
obstacleList = [
412414
(5, 5, 1),
@@ -422,9 +424,10 @@ def main():
422424

423425
# Set Initial parameters
424426
start = [0.0, 0.0, np.deg2rad(0.0)]
425-
goal = [6.0, 7.0, np.deg2rad(90.0)]
427+
goal = [gx, gy, gyaw]
426428

427-
rrt = RRT(start, goal, randArea=[-2.0, 20.0], obstacleList=obstacleList)
429+
rrt = RRT(start, goal, randArea=[-2.0, 20.0],
430+
obstacleList=obstacleList, maxIter=maxIter)
428431
flag, x, y, yaw, v, t, a, d = rrt.Planning(animation=show_animation)
429432

430433
if not flag:

‎PathPlanning/LQRRRTStar/lqr_rrt_star.py

Lines changed: 11 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -12,7 +12,8 @@
1212
import math
1313
import random
1414
import sys
15-
sys.path.append("../LQRPlanner/")
15+
import os
16+
sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../LQRPlanner/")
1617

1718
try:
1819
import LQRplanner
@@ -203,8 +204,8 @@ def calc_dist_to_goal(self, x, y):
203204
def find_near_nodes(self, newNode):
204205
nnode = len(self.nodeList)
205206
r = 50.0 * math.sqrt((math.log(nnode) / nnode))
206-
dlist = [(node.x - newNode.x) ** 2 +
207-
(node.y - newNode.y) ** 2
207+
dlist = [(node.x - newNode.x) ** 2
208+
+ (node.y - newNode.y) ** 2
208209
for node in self.nodeList]
209210
nearinds = [dlist.index(i) for i in dlist if i <= r ** 2]
210211
return nearinds
@@ -246,8 +247,8 @@ def draw_graph(self, rnd=None):
246247
plt.pause(0.01)
247248

248249
def get_nearest_index(self, nodeList, rnd):
249-
dlist = [(node.x - rnd.x) ** 2 +
250-
(node.y - rnd.y) ** 2
250+
dlist = [(node.x - rnd.x) ** 2
251+
+ (node.y - rnd.y) ** 2
251252
for node in nodeList]
252253
minind = dlist.index(min(dlist))
253254

@@ -283,8 +284,8 @@ def __init__(self, x, y):
283284
self.parent = None
284285

285286

286-
def main():
287-
print("Start rrt start planning")
287+
def main(maxIter=200):
288+
print("Start " + __file__)
288289

289290
# ====Search Path with RRT====
290291
obstacleList = [
@@ -300,7 +301,9 @@ def main():
300301
start = [0.0, 0.0]
301302
goal = [6.0, 7.0]
302303

303-
rrt = RRT(start, goal, randArea=[-2.0, 15.0], obstacleList=obstacleList)
304+
rrt = RRT(start, goal, randArea=[-2.0, 15.0],
305+
obstacleList=obstacleList,
306+
maxIter=maxIter)
304307
path = rrt.planning(animation=show_animation)
305308

306309
# Draw final path

‎PathPlanning/RRT/simple_rrt.py

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -145,7 +145,7 @@ def __init__(self, x, y):
145145

146146

147147
def main(gx=5.0, gy=10.0):
148-
print("start simple RRT path planning")
148+
print("start " + __file__)
149149

150150
# ====Search Path with RRT====
151151
obstacleList = [

‎PathPlanning/RRTDubins/rrt_dubins.py

Lines changed: 5 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -10,7 +10,10 @@
1010
import math
1111
import random
1212
import sys
13-
sys.path.append("../DubinsPath/")
13+
import os
14+
sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
15+
"/../DubinsPath/")
16+
1417
try:
1518
import dubins_path_planning
1619
except:
@@ -198,7 +201,7 @@ def __init__(self, x, y, yaw):
198201

199202

200203
def main():
201-
print("Start rrt planning")
204+
print("Start " + __file__)
202205
# ====Search Path with RRT====
203206
obstacleList = [
204207
(5, 5, 1),

‎PathPlanning/RRTStarReedsShepp/rrt_star_reeds_shepp.py

Lines changed: 14 additions & 18 deletions
Original file line numberDiff line numberDiff line change
@@ -11,7 +11,9 @@
1111
import math
1212
import random
1313
import sys
14-
sys.path.append("../ReedsSheppPath/")
14+
import os
15+
sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
16+
"/../PathPlanning/ReedsSheppPath/")
1517

1618
try:
1719
import reeds_shepp_path_planning
@@ -196,9 +198,9 @@ def find_near_nodes(self, newNode):
196198
nnode = len(self.nodeList)
197199
r = 50.0 * math.sqrt((math.log(nnode) / nnode))
198200
# r = self.expandDis * 5.0
199-
dlist = [(node.x - newNode.x) ** 2
200-
+ (node.y - newNode.y) ** 2
201-
+ (node.yaw - newNode.yaw) ** 2
201+
dlist = [(node.x - newNode.x) ** 2 +
202+
(node.y - newNode.y) ** 2 +
203+
(node.yaw - newNode.yaw) ** 2
202204
for node in self.nodeList]
203205
nearinds = [dlist.index(i) for i in dlist if i <= r ** 2]
204206
return nearinds
@@ -249,9 +251,9 @@ def DrawGraph(self, rnd=None):
249251
# input()
250252

251253
def GetNearestListIndex(self, nodeList, rnd):
252-
dlist = [(node.x - rnd.x) ** 2
253-
+ (node.y - rnd.y) ** 2
254-
+ (node.yaw - rnd.yaw) ** 2 for node in nodeList]
254+
dlist = [(node.x - rnd.x) ** 2 +
255+
(node.y - rnd.y) ** 2 +
256+
(node.yaw - rnd.yaw) ** 2 for node in nodeList]
255257
minind = dlist.index(min(dlist))
256258

257259
return minind
@@ -285,18 +287,10 @@ def __init__(self, x, y, yaw):
285287
self.parent = None
286288

287289

288-
def main():
289-
print("Start rrt start planning")
290+
def main(maxIter=200):
291+
print("Start " + __file__)
290292

291293
# ====Search Path with RRT====
292-
# obstacleList = [
293-
# (5, 5, 1),
294-
# (3, 6, 2),
295-
# (3, 8, 2),
296-
# (3, 10, 2),
297-
# (7, 5, 2),
298-
# (9, 5, 2)
299-
# ] # [x,y,size(radius)]
300294
obstacleList = [
301295
(5, 5, 1),
302296
(4, 6, 1),
@@ -313,7 +307,9 @@ def main():
313307
start = [0.0, 0.0, np.deg2rad(0.0)]
314308
goal = [6.0, 7.0, np.deg2rad(90.0)]
315309

316-
rrt = RRT(start, goal, randArea=[-2.0, 15.0], obstacleList=obstacleList)
310+
rrt = RRT(start, goal, randArea=[-2.0, 15.0],
311+
obstacleList=obstacleList,
312+
maxIter=maxIter)
317313
path = rrt.Planning(animation=show_animation)
318314

319315
# Draw final path

‎PathPlanning/RRTstar/rrt_star.py

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -243,7 +243,7 @@ def __init__(self, x, y):
243243

244244

245245
def main():
246-
print("Start rrt planning")
246+
print("Start " + __file__)
247247

248248
# ====Search Path with RRT====
249249
obstacleList = [

‎PathPlanning/StateLatticePlanner/state_lattice_planner.py

Lines changed: 13 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -6,17 +6,24 @@
66
77
"""
88
import sys
9-
10-
sys.path.append("../ModelPredictiveTrajectoryGenerator")
11-
9+
import os
1210
from matplotlib import pyplot as plt
1311
import numpy as np
1412
import math
1513
import pandas as pd
16-
import model_predictive_trajectory_generator as planner
17-
import motion_model
1814

19-
table_path = "./lookuptable.csv"
15+
sys.path.append(os.path.dirname(os.path.abspath(__file__))
16+
+ "/../ModelPredictiveTrajectoryGenerator/")
17+
18+
19+
try:
20+
import model_predictive_trajectory_generator as planner
21+
import motion_model
22+
except:
23+
raise
24+
25+
26+
table_path = os.path.dirname(os.path.abspath(__file__)) + "/lookuptable.csv"
2027

2128
show_animation = True
2229

‎tests/test_closed_loop_rrt_star_car.py

Lines changed: 15 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -1,6 +1,14 @@
11
from unittest import TestCase
2+
import sys
3+
import os
4+
sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../")
5+
sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
6+
"/../PathPlanning/ClosedLoopRRTStar/")
7+
try:
8+
from PathPlanning.ClosedLoopRRTStar import closed_loop_rrt_star_car as m
9+
except:
10+
raise
211

3-
from SLAM.iterative_closest_point import iterative_closest_point as m
412

513
print(__file__)
614

@@ -9,4 +17,9 @@ class Test(TestCase):
917

1018
def test1(self):
1119
m.show_animation = False
12-
m.main()
20+
m.main(gx=1.0, gy=0.0, gyaw=0.0, maxIter=5)
21+
22+
23+
if __name__ == '__main__':
24+
test = Test()
25+
test.test1()

‎tests/test_informed_rrt_star.py

Lines changed: 14 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -1,6 +1,13 @@
11
from unittest import TestCase
2-
3-
from PathPlanning.InformedRRTStar import informed_rrt_star as m
2+
import sys
3+
import os
4+
sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../")
5+
sys.path.append(os.path.dirname(os.path.abspath(__file__))
6+
+ "/../PathPlanning/InformedRRTStar/")
7+
try:
8+
from PathPlanning.InformedRRTStar import informed_rrt_star as m
9+
except:
10+
raise
411

512
print(__file__)
613

@@ -10,3 +17,8 @@ class Test(TestCase):
1017
def test1(self):
1118
m.show_animation = False
1219
m.main()
20+
21+
22+
if __name__ == '__main__':
23+
test = Test()
24+
test.test1()

‎tests/test_lqr_rrt_star.py

Lines changed: 14 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -1,9 +1,13 @@
11
from unittest import TestCase
2-
32
import sys
4-
sys.path.append("./PathPlanning/LQRRRTStar/")
5-
6-
from PathPlanning.LQRRRTStar import lqr_rrt_star as m
3+
import os
4+
sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../")
5+
sys.path.append(os.path.dirname(os.path.abspath(__file__))
6+
+ "/../PathPlanning/LQRRRTStar/")
7+
try:
8+
from PathPlanning.LQRRRTStar import lqr_rrt_star as m
9+
except:
10+
raise
711

812
print(__file__)
913

@@ -12,4 +16,9 @@ class Test(TestCase):
1216

1317
def test1(self):
1418
m.show_animation = False
15-
m.main()
19+
m.main(maxIter=5)
20+
21+
22+
if __name__ == '__main__':
23+
test = Test()
24+
test.test1()

‎tests/test_rrt_dubins.py

Lines changed: 11 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -1,11 +1,13 @@
11
from unittest import TestCase
22
import sys
3-
sys.path.append("./PathPlanning/RRTDubins/")
4-
sys.path.append("./PathPlanning/DubinsPath/")
3+
import os
4+
sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
5+
"/../PathPlanning/RRTDubins/")
6+
sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
7+
"/../PathPlanning/DubinsPath/")
58

69
try:
7-
from PathPlanning.RRTDubins import rrt_dubins as m
8-
# from RRTDubins import rrt_dubins as m
10+
import rrt_dubins as m
911
except:
1012
raise
1113

@@ -18,3 +20,8 @@ class Test(TestCase):
1820
def test1(self):
1921
m.show_animation = False
2022
m.main()
23+
24+
25+
if __name__ == '__main__':
26+
test = Test()
27+
test.test1()

‎tests/test_rrt_star.py

Lines changed: 14 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,6 +1,14 @@
11
from unittest import TestCase
2+
import sys
3+
import os
4+
sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
5+
"/../PathPlanning/RRTStar/")
6+
7+
try:
8+
import rrt_star as m
9+
except:
10+
raise
211

3-
from PathPlanning.RRTstar import rrt_star as m
412

513
print(__file__)
614

@@ -10,3 +18,8 @@ class Test(TestCase):
1018
def test1(self):
1119
m.show_animation = False
1220
m.main()
21+
22+
23+
if __name__ == '__main__':
24+
test = Test()
25+
test.test1()

‎tests/test_rrt_star_dubins.py

Lines changed: 12 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -1,9 +1,14 @@
11
from unittest import TestCase
22

33
import sys
4-
sys.path.append("./PathPlanning/RRTStarDubins/")
4+
import os
5+
sys.path.append(os.path.dirname(os.path.abspath(__file__))
6+
+ "/../PathPlanning/RRTStarDubins/")
57

6-
from PathPlanning.RRTStarDubins import rrt_star_dubins as m
8+
try:
9+
import rrt_star_dubins as m
10+
except:
11+
raise
712

813
print(__file__)
914

@@ -13,3 +18,8 @@ class Test(TestCase):
1318
def test1(self):
1419
m.show_animation = False
1520
m.main()
21+
22+
23+
if __name__ == '__main__':
24+
test = Test()
25+
test.test1()

0 commit comments

Comments
 (0)