Skip to content

Commit d109460

Browse files
AtsushiSakaiAtsushiSakai
committed
fix bug of RRT and added test code
1 parent 53381e9 commit d109460

4 files changed

Lines changed: 126 additions & 169 deletions

File tree

‎PathPlanning/RRT/matplotrecorder.py‎

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

‎PathPlanning/RRT/rrt_with_pathsmoothing.py‎

Lines changed: 87 additions & 82 deletions
Original file line numberDiff line numberDiff line change
@@ -1,25 +1,25 @@
1-
#!/usr/bin/python
2-
# -*- coding: utf-8 -*-
3-
u"""
4-
@brief: Path Planning Sample Code with Randamized Rapidly-Exploring Random Trees (RRT)
5-
6-
@author: AtsushiSakai
1+
"""
2+
Path Planning Sample Code with Randamized Rapidly-Exploring Random Trees (RRT)
73
8-
@license: MIT
4+
@author: AtsushiSakai(@Atsushi_twi)
95
106
"""
117

8+
import matplotlib.pyplot as plt
129
import random
1310
import math
1411
import copy
1512

13+
show_animation = True
14+
15+
1616
class RRT():
17-
u"""
17+
"""
1818
Class for RRT Planning
1919
"""
2020

21-
def __init__(self, start, goal, obstacleList,randArea,expandDis=1.0,goalSampleRate=5,maxIter=500):
22-
u"""
21+
def __init__(self, start, goal, obstacleList, randArea, expandDis=1.0, goalSampleRate=5, maxIter=500):
22+
"""
2323
Setting Parameter
2424
2525
start:Start Position [x,y]
@@ -28,17 +28,18 @@ def __init__(self, start, goal, obstacleList,randArea,expandDis=1.0,goalSampleRa
2828
randArea:Ramdom Samping Area [min,max]
2929
3030
"""
31-
self.start=Node(start[0],start[1])
32-
self.end=Node(goal[0],goal[1])
31+
self.start = Node(start[0], start[1])
32+
self.end = Node(goal[0], goal[1])
3333
self.minrand = randArea[0]
3434
self.maxrand = randArea[1]
3535
self.expandDis = expandDis
3636
self.goalSampleRate = goalSampleRate
3737
self.maxIter = maxIter
38+
self.obstacleList = obstacleList
3839

39-
def Planning(self,animation=True):
40-
u"""
41-
Pathplanning
40+
def Planning(self, animation=True):
41+
"""
42+
Pathplanning
4243
4344
animation: flag for animation on or off
4445
"""
@@ -47,7 +48,8 @@ def Planning(self,animation=True):
4748
while True:
4849
# Random Sampling
4950
if random.randint(0, 100) > self.goalSampleRate:
50-
rnd = [random.uniform(self.minrand, self.maxrand), random.uniform(self.minrand, self.maxrand)]
51+
rnd = [random.uniform(self.minrand, self.maxrand), random.uniform(
52+
self.minrand, self.maxrand)]
5153
else:
5254
rnd = [self.end.x, self.end.y]
5355

@@ -56,15 +58,15 @@ def Planning(self,animation=True):
5658
# print(nind)
5759

5860
# expand tree
59-
nearestNode =self.nodeList[nind]
61+
nearestNode = self.nodeList[nind]
6062
theta = math.atan2(rnd[1] - nearestNode.y, rnd[0] - nearestNode.x)
6163

6264
newNode = copy.deepcopy(nearestNode)
6365
newNode.x += self.expandDis * math.cos(theta)
6466
newNode.y += self.expandDis * math.sin(theta)
6567
newNode.parent = nind
6668

67-
if not self.__CollisionCheck(newNode, obstacleList):
69+
if not self.__CollisionCheck(newNode, self.obstacleList):
6870
continue
6971

7072
self.nodeList.append(newNode)
@@ -80,43 +82,43 @@ def Planning(self,animation=True):
8082
if animation:
8183
self.DrawGraph(rnd)
8284

83-
84-
path=[[self.end.x,self.end.y]]
85+
path = [[self.end.x, self.end.y]]
8586
lastIndex = len(self.nodeList) - 1
8687
while self.nodeList[lastIndex].parent is not None:
8788
node = self.nodeList[lastIndex]
88-
path.append([node.x,node.y])
89+
path.append([node.x, node.y])
8990
lastIndex = node.parent
9091
path.append([self.start.x, self.start.y])
9192

9293
return path
9394

94-
def DrawGraph(self,rnd=None):
95-
import matplotlib.pyplot as plt
95+
def DrawGraph(self, rnd=None):
9696
plt.clf()
9797
if rnd is not None:
9898
plt.plot(rnd[0], rnd[1], "^k")
9999
for node in self.nodeList:
100100
if node.parent is not None:
101-
plt.plot([node.x, self.nodeList[node.parent].x], [node.y, self.nodeList[node.parent].y], "-g")
102-
for (x,y,size) in obstacleList:
103-
self.PlotCircle(x,y,size)
101+
plt.plot([node.x, self.nodeList[node.parent].x], [
102+
node.y, self.nodeList[node.parent].y], "-g")
103+
for (x, y, size) in self.obstacleList:
104+
self.PlotCircle(x, y, size)
104105

105106
plt.plot(self.start.x, self.start.y, "xr")
106107
plt.plot(self.end.x, self.end.y, "xr")
107108
plt.axis([-2, 15, -2, 15])
108109
plt.grid(True)
109110
plt.pause(0.01)
110111

111-
def PlotCircle(self,x,y,size):
112-
deg=range(0,360,5)
112+
def PlotCircle(self, x, y, size):
113+
deg = list(range(0, 360, 5))
113114
deg.append(0)
114-
xl=[x+size*math.cos(math.radians(d)) for d in deg]
115-
yl=[y+size*math.sin(math.radians(d)) for d in deg]
115+
xl = [x + size * math.cos(math.radians(d)) for d in deg]
116+
yl = [y + size * math.sin(math.radians(d)) for d in deg]
116117
plt.plot(xl, yl, "-k")
117118

118119
def GetNearestListIndex(self, nodeList, rnd):
119-
dlist = [(node.x - rnd[0]) ** 2 + (node.y - rnd[1]) ** 2 for node in nodeList]
120+
dlist = [(node.x - rnd[0]) ** 2 + (node.y - rnd[1])
121+
** 2 for node in nodeList]
120122
minind = dlist.index(min(dlist))
121123
return minind
122124

@@ -131,8 +133,9 @@ def __CollisionCheck(self, node, obstacleList):
131133

132134
return True # safe
133135

136+
134137
class Node():
135-
u"""
138+
"""
136139
RRT Node
137140
"""
138141

@@ -143,31 +146,31 @@ def __init__(self, x, y):
143146

144147

145148
def GetPathLength(path):
146-
l = 0
149+
le = 0
147150
for i in range(len(path) - 1):
148151
dx = path[i + 1][0] - path[i][0]
149152
dy = path[i + 1][1] - path[i][1]
150153
d = math.sqrt(dx * dx + dy * dy)
151-
l += d
154+
le += d
152155

153-
return l
156+
return le
154157

155158

156159
def GetTargetPoint(path, targetL):
157-
l = 0
160+
le = 0
158161
ti = 0
159162
lastPairLen = 0
160163
for i in range(len(path) - 1):
161164
dx = path[i + 1][0] - path[i][0]
162165
dy = path[i + 1][1] - path[i][1]
163166
d = math.sqrt(dx * dx + dy * dy)
164-
l += d
165-
if l >= targetL:
166-
ti = i-1
167+
le += d
168+
if le >= targetL:
169+
ti = i - 1
167170
lastPairLen = d
168171
break
169172

170-
partRatio = (l - targetL) / lastPairLen
173+
partRatio = (le - targetL) / lastPairLen
171174
# print(partRatio)
172175
# print((ti,len(path),path[ti],path[ti+1]))
173176

@@ -181,26 +184,21 @@ def GetTargetPoint(path, targetL):
181184
def LineCollisionCheck(first, second, obstacleList):
182185
# Line Equation
183186

184-
x1=first[0]
185-
y1=first[1]
186-
x2=second[0]
187-
y2=second[1]
187+
x1 = first[0]
188+
y1 = first[1]
189+
x2 = second[0]
190+
y2 = second[1]
188191

189192
try:
190-
a=y2-y1
191-
b=-(x2-x1)
192-
c=y2*(x2-x1)-x2*(y2-y1)
193+
a = y2 - y1
194+
b = -(x2 - x1)
195+
c = y2 * (x2 - x1) - x2 * (y2 - y1)
193196
except ZeroDivisionError:
194197
return False
195198

196-
# print(first)
197-
# print(second)
198-
199-
for (ox,oy,size) in obstacleList:
200-
d=abs(a*ox+b*oy+c)/(math.sqrt(a*a+b*b))
201-
# print((ox,oy,size,d))
202-
if d<=(size):
203-
# print("NG")
199+
for (ox, oy, size) in obstacleList:
200+
d = abs(a * ox + b * oy + c) / (math.sqrt(a * a + b * b))
201+
if d <= (size):
204202
return False
205203

206204
# print("OK")
@@ -211,46 +209,45 @@ def LineCollisionCheck(first, second, obstacleList):
211209
def PathSmoothing(path, maxIter, obstacleList):
212210
# print("PathSmoothing")
213211

214-
l = GetPathLength(path)
212+
le = GetPathLength(path)
215213

216214
for i in range(maxIter):
217215
# Sample two points
218-
pickPoints = [random.uniform(0, l), random.uniform(0, l)]
216+
pickPoints = [random.uniform(0, le), random.uniform(0, le)]
219217
pickPoints.sort()
220218
# print(pickPoints)
221219
first = GetTargetPoint(path, pickPoints[0])
222220
# print(first)
223221
second = GetTargetPoint(path, pickPoints[1])
224222
# print(second)
225223

226-
if first[2]<=0 or second[2]<=0:
224+
if first[2] <= 0 or second[2] <= 0:
227225
continue
228226

229-
if (second[2]+1) > len(path):
227+
if (second[2] + 1) > len(path):
230228
continue
231229

232-
if second[2]==first[2]:
230+
if second[2] == first[2]:
233231
continue
234232

235233
# collision check
236234
if not LineCollisionCheck(first, second, obstacleList):
237235
continue
238236

239-
#Create New path
240-
newPath=[]
241-
newPath.extend(path[:first[2]+1])
242-
newPath.append([first[0],first[1]])
243-
newPath.append([second[0],second[1]])
244-
newPath.extend(path[second[2]+1:])
245-
path=newPath
246-
l = GetPathLength(path)
237+
# Create New path
238+
newPath = []
239+
newPath.extend(path[:first[2] + 1])
240+
newPath.append([first[0], first[1]])
241+
newPath.append([second[0], second[1]])
242+
newPath.extend(path[second[2] + 1:])
243+
path = newPath
244+
le = GetPathLength(path)
247245

248246
return path
249247

250248

251-
if __name__ == '__main__':
252-
import matplotlib.pyplot as plt
253-
#====Search Path with RRT====
249+
def main():
250+
# ====Search Path with RRT====
254251
# Parameter
255252
obstacleList = [
256253
(5, 5, 1),
@@ -260,18 +257,26 @@ def PathSmoothing(path, maxIter, obstacleList):
260257
(7, 5, 2),
261258
(9, 5, 2)
262259
] # [x,y,size]
263-
rrt=RRT(start=[0,0],goal=[5,10],randArea=[-2,15],obstacleList=obstacleList)
264-
path=rrt.Planning(animation=True)
260+
rrt = RRT(start=[0, 0], goal=[5, 10],
261+
randArea=[-2, 15], obstacleList=obstacleList)
262+
path = rrt.Planning(animation=show_animation)
263+
264+
# Path smoothing
265+
maxIter = 1000
266+
smoothedPath = PathSmoothing(path, maxIter, obstacleList)
265267

266268
# Draw final path
267-
rrt.DrawGraph()
268-
plt.plot([x for (x,y) in path], [y for (x,y) in path],'-r')
269+
if show_animation:
270+
rrt.DrawGraph()
271+
plt.plot([x for (x, y) in path], [y for (x, y) in path], '-r')
269272

270-
#Path smoothing
271-
maxIter=1000
272-
smoothedPath = PathSmoothing(path, maxIter, obstacleList)
273-
plt.plot([x for (x,y) in smoothedPath], [y for (x,y) in smoothedPath],'-b')
273+
plt.plot([x for (x, y) in smoothedPath], [
274+
y for (x, y) in smoothedPath], '-b')
274275

275-
plt.grid(True)
276-
plt.pause(0.01) # Need for Mac
277-
plt.show()
276+
plt.grid(True)
277+
plt.pause(0.01) # Need for Mac
278+
plt.show()
279+
280+
281+
if __name__ == '__main__':
282+
main()

0 commit comments

Comments
 (0)