add new sample

scripts/PathPlanning/RRT/rrt_with_pathsmoothing.py

@@ -0,0 +1,277 @@
+#!/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()

scripts/PathPlanning/RRT/simple_rrt.py

@@ -9,6 +9,7 @@ u"""
 
 """
 
+
 import random
 import math
 import copy