#!/usr/bin/python # -*- coding: utf-8 -*- u""" @brief: Path Planning Sample Code with Randamized Rapidly-Exploring Random Trees (RRT) @author: AtsushiSakai @license: MIT """ import random import math import copy class RRT(): u""" Class for RRT Planning """ def __init__(self, start, goal, obstacleList,randArea,expandDis=1.0,goalSampleRate=5,maxIter=500): u""" Setting Parameter start:Start Position [x,y] goal:Goal Position [x,y] obstacleList:obstacle Positions [[x,y,size],...] randArea:Ramdom Samping Area [min,max] """ self.start=Node(start[0],start[1]) self.end=Node(goal[0],goal[1]) self.minrand = randArea[0] self.maxrand = randArea[1] self.expandDis = expandDis self.goalSampleRate = goalSampleRate self.maxIter = maxIter def Planning(self,animation=True): u""" Pathplanning animation: flag for animation on or off """ self.nodeList = [self.start] while True: # Random Sampling if random.randint(0, 100) > self.goalSampleRate: rnd = [random.uniform(self.minrand, self.maxrand), random.uniform(self.minrand, self.maxrand)] else: rnd = [self.end.x, self.end.y] # Find nearest node nind = self.GetNearestListIndex(self.nodeList, rnd) # print(nind) # expand tree nearestNode =self.nodeList[nind] theta = math.atan2(rnd[1] - nearestNode.y, rnd[0] - nearestNode.x) newNode = copy.deepcopy(nearestNode) newNode.x += self.expandDis * math.cos(theta) newNode.y += self.expandDis * math.sin(theta) newNode.parent = nind if not self.__CollisionCheck(newNode, obstacleList): continue self.nodeList.append(newNode) # check goal dx = newNode.x - self.end.x dy = newNode.y - self.end.y d = math.sqrt(dx * dx + dy * dy) if d <= self.expandDis: print("Goal!!") break if animation: self.DrawGraph(rnd) path=[[self.end.x,self.end.y]] lastIndex = len(self.nodeList) - 1 while self.nodeList[lastIndex].parent is not None: node = self.nodeList[lastIndex] path.append([node.x,node.y]) lastIndex = node.parent path.append([self.start.x, self.start.y]) return path def DrawGraph(self,rnd=None): import matplotlib.pyplot as plt plt.clf() if rnd is not None: plt.plot(rnd[0], rnd[1], "^k") for node in self.nodeList: if node.parent is not None: plt.plot([node.x, self.nodeList[node.parent].x], [node.y, self.nodeList[node.parent].y], "-g") for (x,y,size) in obstacleList: self.PlotCircle(x,y,size) plt.plot(self.start.x, self.start.y, "xr") plt.plot(self.end.x, self.end.y, "xr") plt.axis([-2, 15, -2, 15]) plt.grid(True) plt.pause(0.01) def PlotCircle(self,x,y,size): deg=range(0,360,5) deg.append(0) xl=[x+size*math.cos(math.radians(d)) for d in deg] yl=[y+size*math.sin(math.radians(d)) for d in deg] plt.plot(xl, yl, "-k") def GetNearestListIndex(self, nodeList, rnd): dlist = [(node.x - rnd[0]) ** 2 + (node.y - rnd[1]) ** 2 for node in nodeList] minind = dlist.index(min(dlist)) return minind def __CollisionCheck(self, node, obstacleList): for (ox, oy, size) in obstacleList: dx = ox - node.x dy = oy - node.y d = math.sqrt(dx * dx + dy * dy) if d <= size: return False # collision return True # safe class Node(): u""" RRT Node """ def __init__(self, x, y): self.x = x self.y = y self.parent = None def GetPathLength(path): l = 0 for i in range(len(path) - 1): dx = path[i + 1][0] - path[i][0] dy = path[i + 1][1] - path[i][1] d = math.sqrt(dx * dx + dy * dy) l += d return l def GetTargetPoint(path, targetL): l = 0 ti = 0 lastPairLen = 0 for i in range(len(path) - 1): dx = path[i + 1][0] - path[i][0] dy = path[i + 1][1] - path[i][1] d = math.sqrt(dx * dx + dy * dy) l += d if l >= targetL: ti = i-1 lastPairLen = d break partRatio = (l - targetL) / lastPairLen # print(partRatio) # print((ti,len(path),path[ti],path[ti+1])) x = path[ti][0] + (path[ti + 1][0] - path[ti][0]) * partRatio y = path[ti][1] + (path[ti + 1][1] - path[ti][1]) * partRatio # print((x,y)) return [x, y, ti] def LineCollisionCheck(first, second, obstacleList): # Line Equation x1=first[0] y1=first[1] x2=second[0] y2=second[1] try: a=y2-y1 b=-(x2-x1) c=y2*(x2-x1)-x2*(y2-y1) except ZeroDivisionError: return False # print(first) # print(second) for (ox,oy,size) in obstacleList: d=abs(a*ox+b*oy+c)/(math.sqrt(a*a+b*b)) # print((ox,oy,size,d)) if d<=(size): # print("NG") return False # print("OK") return True # OK def PathSmoothing(path, maxIter, obstacleList): # print("PathSmoothing") l = GetPathLength(path) for i in range(maxIter): # Sample two points pickPoints = [random.uniform(0, l), random.uniform(0, l)] pickPoints.sort() # print(pickPoints) first = GetTargetPoint(path, pickPoints[0]) # print(first) second = GetTargetPoint(path, pickPoints[1]) # print(second) if first[2]<=0 or second[2]<=0: continue if (second[2]+1) > len(path): continue if second[2]==first[2]: continue # collision check if not LineCollisionCheck(first, second, obstacleList): continue #Create New path newPath=[] newPath.extend(path[:first[2]+1]) newPath.append([first[0],first[1]]) newPath.append([second[0],second[1]]) newPath.extend(path[second[2]+1:]) path=newPath l = GetPathLength(path) return path if __name__ == '__main__': import matplotlib.pyplot as plt #====Search Path with RRT==== # Parameter obstacleList = [ (5, 5, 1), (3, 6, 2), (3, 8, 2), (3, 10, 2), (7, 5, 2), (9, 5, 2) ] # [x,y,size] rrt=RRT(start=[0,0],goal=[5,10],randArea=[-2,15],obstacleList=obstacleList) path=rrt.Planning(animation=True) # Draw final path rrt.DrawGraph() plt.plot([x for (x,y) in path], [y for (x,y) in path],'-r') #Path smoothing maxIter=1000 smoothedPath = PathSmoothing(path, maxIter, obstacleList) plt.plot([x for (x,y) in smoothedPath], [y for (x,y) in smoothedPath],'-b') plt.grid(True) plt.pause(0.01) # Need for Mac plt.show()