Skip to content

Commit fa3692d

Browse files
AtsushiSakaiAtsushiSakai
committed
test fixed
1 parent 0b1ca74 commit fa3692d

7 files changed

Lines changed: 46 additions & 100 deletions

File tree

‎PathPlanning/ClosedLoopRRTStar/closed_loop_rrt_star_car.py‎

Lines changed: 21 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -21,7 +21,7 @@
2121

2222

2323
target_speed = 10.0 / 3.6
24-
STEP_SIZE = 0.5
24+
STEP_SIZE = 0.1
2525

2626

2727
class RRT():
@@ -52,6 +52,8 @@ def try_goal_path(self):
5252
goal = Node(self.end.x, self.end.y, self.end.yaw)
5353

5454
newNode = self.steer(goal, len(self.nodeList) - 1)
55+
if newNode is None:
56+
return
5557

5658
if self.CollisionCheck(newNode, self.obstacleList):
5759
# print("goal path is OK")
@@ -74,10 +76,14 @@ def Planning(self, animation=True):
7476

7577
newNode = self.steer(rnd, nind)
7678
# print(newNode.cost)
79+
if newNode is None:
80+
continue
7781

7882
if self.CollisionCheck(newNode, self.obstacleList):
7983
nearinds = self.find_near_nodes(newNode)
8084
newNode = self.choose_parent(newNode, nearinds)
85+
if newNode is None:
86+
continue
8187

8288
self.nodeList.append(newNode)
8389
self.rewire(newNode, nearinds)
@@ -97,6 +103,8 @@ def Planning(self, animation=True):
97103

98104
def search_best_feasible_path(self, path_indexs):
99105

106+
print("Start search feasible path")
107+
100108
best_time = float("inf")
101109

102110
# pure pursuit tracking
@@ -189,6 +197,9 @@ def choose_parent(self, newNode, nearinds):
189197
dlist = []
190198
for i in nearinds:
191199
tNode = self.steer(newNode, i)
200+
if tNode is None:
201+
continue
202+
192203
if self.CollisionCheck(tNode, self.obstacleList):
193204
dlist.append(tNode.cost)
194205
else:
@@ -202,6 +213,8 @@ def choose_parent(self, newNode, nearinds):
202213
return newNode
203214

204215
newNode = self.steer(newNode, minind)
216+
if newNode is None:
217+
return None
205218

206219
return newNode
207220

@@ -223,6 +236,9 @@ def steer(self, rnd, nind):
223236
nearestNode.x, nearestNode.y, nearestNode.yaw,
224237
rnd.x, rnd.y, rnd.yaw, unicycle_model.curvature_max, STEP_SIZE)
225238

239+
if px is None:
240+
return None
241+
226242
newNode = copy.deepcopy(nearestNode)
227243
newNode.x = px[-1]
228244
newNode.y = py[-1]
@@ -231,7 +247,7 @@ def steer(self, rnd, nind):
231247
newNode.path_x = px
232248
newNode.path_y = py
233249
newNode.path_yaw = pyaw
234-
newNode.cost += sum(clen)
250+
newNode.cost += sum([abs(c) for c in clen])
235251
newNode.parent = nind
236252

237253
return newNode
@@ -308,6 +324,9 @@ def rewire(self, newNode, nearinds):
308324
nearNode = self.nodeList[i]
309325
tNode = self.steer(nearNode, nnode - 1)
310326

327+
if tNode is None:
328+
continue
329+
311330
obstacleOK = self.CollisionCheck(tNode, self.obstacleList)
312331
imporveCost = nearNode.cost > tNode.cost
313332

‎PathPlanning/RRTStarReedsShepp/reeds_shepp_path_planning.py‎

Lines changed: 0 additions & 92 deletions
This file was deleted.

‎PathPlanning/RRTStarReedsShepp/rrt_star_reeds_shepp.py‎

Lines changed: 19 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -6,6 +6,9 @@
66
77
"""
88

9+
import sys
10+
sys.path.append("../ReedsSheppPath/")
11+
912
import random
1013
import math
1114
import copy
@@ -14,6 +17,8 @@
1417
import matplotlib.pyplot as plt
1518

1619
show_animation = True
20+
STEP_SIZE = 0.1
21+
curvature = 1.0
1722

1823

1924
class RRT():
@@ -53,10 +58,14 @@ def Planning(self, animation=True):
5358
nind = self.GetNearestListIndex(self.nodeList, rnd)
5459

5560
newNode = self.steer(rnd, nind)
61+
if newNode is None:
62+
continue
5663

5764
if self.CollisionCheck(newNode, self.obstacleList):
5865
nearinds = self.find_near_nodes(newNode)
5966
newNode = self.choose_parent(newNode, nearinds)
67+
if newNode is None:
68+
continue
6069
self.nodeList.append(newNode)
6170
self.rewire(newNode, nearinds)
6271

@@ -77,6 +86,9 @@ def choose_parent(self, newNode, nearinds):
7786
dlist = []
7887
for i in nearinds:
7988
tNode = self.steer(newNode, i)
89+
if tNode is None:
90+
continue
91+
8092
if self.CollisionCheck(tNode, self.obstacleList):
8193
dlist.append(tNode.cost)
8294
else:
@@ -103,12 +115,14 @@ def pi_2_pi(self, angle):
103115
return angle
104116

105117
def steer(self, rnd, nind):
106-
curvature = 1.0
107118

108119
nearestNode = self.nodeList[nind]
109120

110121
px, py, pyaw, mode, clen = reeds_shepp_path_planning.reeds_shepp_path_planning(
111-
nearestNode.x, nearestNode.y, nearestNode.yaw, rnd.x, rnd.y, rnd.yaw, curvature)
122+
nearestNode.x, nearestNode.y, nearestNode.yaw, rnd.x, rnd.y, rnd.yaw, curvature, STEP_SIZE)
123+
124+
if px is None:
125+
return None
112126

113127
newNode = copy.deepcopy(nearestNode)
114128
newNode.x = px[-1]
@@ -118,7 +132,7 @@ def steer(self, rnd, nind):
118132
newNode.path_x = px
119133
newNode.path_y = py
120134
newNode.path_yaw = pyaw
121-
newNode.cost += clen
135+
newNode.cost += sum([abs(c) for c in clen])
122136
newNode.parent = nind
123137

124138
return newNode
@@ -200,6 +214,8 @@ def rewire(self, newNode, nearinds):
200214
for i in nearinds:
201215
nearNode = self.nodeList[i]
202216
tNode = self.steer(nearNode, nnode - 1)
217+
if tNode is None:
218+
continue
203219

204220
obstacleOK = self.CollisionCheck(tNode, self.obstacleList)
205221
imporveCost = nearNode.cost > tNode.cost

‎PathPlanning/RRTstar/rrt_star.py‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -65,6 +65,8 @@ def Planning(self, animation=True):
6565

6666
# generate coruse
6767
lastIndex = self.get_best_last_index()
68+
if lastIndex is None:
69+
return None
6870
path = self.gen_final_course(lastIndex)
6971
return path
7072

‎PathPlanning/ReedsSheppPath/reeds_shepp_path_planning.py‎

Lines changed: 3 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -369,7 +369,8 @@ def reeds_shepp_path_planning(sx, sy, syaw,
369369
paths = calc_paths(sx, sy, syaw, gx, gy, gyaw, maxc, step_size)
370370

371371
if len(paths) == 0:
372-
print("No path")
372+
# print("No path")
373+
# print(sx, sy, syaw, gx, gy, gyaw)
373374
return None, None, None, None, None
374375

375376
minL = float("Inf")
@@ -443,7 +444,7 @@ def main():
443444
start_x, start_y, start_yaw, end_x, end_y, end_yaw, curvature, step_size)
444445

445446
if show_animation:
446-
plt.plot(px, py, label="final course " + str(mode))
447+
# plt.plot(px, py, label="final course " + str(mode))
447448

448449
# plotting
449450
plot_arrow(start_x, start_y, start_yaw)

‎tests/test_reeds_shepp_path_planning.py‎

Lines changed: 0 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -7,4 +7,3 @@ class Test(TestCase):
77
def test1(self):
88
m.show_animation = False
99
m.main()
10-
m.test()

‎tests/test_rrt_star_reeds_shepp.py‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -2,6 +2,7 @@
22

33
import sys
44
sys.path.append("./PathPlanning/RRTStarReedsShepp/")
5+
sys.path.append("./PathPlanning/ReedsSheppPath/")
56

67
from PathPlanning.RRTStarReedsShepp import rrt_star_reeds_shepp as m
78

0 commit comments

Comments
 (0)