自动驾驶算法（三）：RRT算法讲解与代码实现（基于采样的路径规划）

目录

1 RRT算法原理

2 RRT算法代码解析

3 RRT完整代码

1 RRT算法原理

        RRT算法的全称是快速扩展随机树算法(Rapidly Exploring Random Tree)，它的想法就是从根结点长出一棵树当树枝长到终点的时候这样就能找到从终点到根节点的唯一路径。

        算法流程：

        首先进行初始化设置起点和终点的设置，进入循环，进行随机采样在空间中随机选择一个点Xrand，寻找距离Xrand最近的节点（如果是第一次那就是初始节点），随后我们进行树的生长，将Xnear--Xrand连接起来作为树生长的方向，设置步长作为树枝的长度，就会产生Xnew节点，如下图：

        将树枝节点和根结点加在一起作为新节点。

        我们继续来采样：这次采样也是Xrand

        它最近的节点是Xnear节点，生长的时候会发现它会穿越障碍物。抛弃这次的生长。

        在不停的生长过后，Xnew越来越接近终点。我们每次产生Xnew'都会与终点进行连线，看他们的距离是否比步长小并且没有障碍物，如果true，我们连接Xnew和终点就找到了起点到终点的路径。

        我们执行代码：

        红色的就是我们找到的路径。好像挺糟糕的.....

2 RRT算法代码解析

        我们先看算法的节点类：

class Node:
 
    def __init__(self, x, y):
        self.x = x
        self.y = y
        self.cost = 0.0
        self.parent = None

        和Dijkstra和A*算法是一样的，不过这里的x、y是真实坐标系的坐标而不是珊格地图的坐标，节点代价为距离，如果连接了下一个节点这个代价也是进行相加的，parent存储了父亲节点，方便于找到终点后计算路径。

        我们再看准备工作：

    obstacleList = [(5, 5, 1), (3, 6, 2), (3, 8, 2), (3, 10, 2), (7, 5, 2),
                    (9, 5, 2), (8, 10, 1)]
 
    # Set params
    # 采样范围 设置的障碍物 最大迭代次数
    rrt = RRT(randArea=[-2, 18], obstacleList=obstacleList, maxIter=200)
    # 传入的是起点和终点
    path = rrt.rrt_planning(start=[0, 0], goal=[15, 12], animation=show_animation)

        我们设计了一系列的圆柱障碍物，并向RRT类中传参，圆柱障碍物是中心坐标+半径的形式，如下图：

        由上面的算法介绍可知，我们传入RRT算法的参数为障碍物、随机距离（-2-18）、最大迭代次数（如果超过200次我就不找路径了），我们看RRT类，首先看构造函数：

    def __init__(self, obstacleList, randArea,
                 expandDis=2.0, goalSampleRate=10, maxIter=200):
 
        self.start = None
        self.goal = None
        self.min_rand = randArea[0]
        self.max_rand = randArea[1]
        self.expand_dis = expandDis
        self.goal_sample_rate = goalSampleRate
        self.max_iter = maxIter
        self.obstacle_list = obstacleList
        # 存储RRT树
        self.node_list = None

        将起始点、结束点置为null，最小随机点和最大随机取样点设置为-2与18，单次前进距离（X_near --> X_rand）为2，直接取终点为最终点的采样概率为10%，最大迭代次数为200，障碍物列表也传进来了。RRT树为none。

        我们看导航参数：

path = rrt.rrt_planning(start=[0, 0], goal=[15, 12], animation=show_animation)

        就是输入了起点和终点的坐标。

        我们进入函数：

    def rrt_planning(self, start, goal, animation=True):
        start_time = time.time()
        self.start = Node(start[0], start[1])
        self.goal = Node(goal[0], goal[1])
        # 将起点加入node_list作为树的根结点
        self.node_list = [self.start]
        path = None

        记录了起始时间计算时间，我们将起始点和终止点封装成node，因为我们没有指定代价cost和parent，因此代价由类默认设置为0，父亲节点设置为-1。将起点加入到node_list中为根结点。初始化路径。       

        我们进行采样：
 

        for i in range(self.max_iter):
            # 进行采样
            rnd = self.sample()
            # 取的距离采样点最近的节点下标
            n_ind = self.get_nearest_list_index(self.node_list, rnd)
            # 得到最近节点
            nearestNode = self.node_list[n_ind]
 
            # 将Xrandom和Xnear连线方向作为生长方向
            # math.atan2() 函数接受两个参数，分别是 y 坐标差值和 x 坐标差值。它返回的值是以弧度表示的角度，范围在 -π 到 π 之间。这个角度表示了从 nearestNode 指向 rnd 的方向。
            theta = math.atan2(rnd[1] - nearestNode.y, rnd[0] - nearestNode.x)
            # 生长 ： 输入参数为角度、下标、nodelist中最近的节点  得到生长过后的节点
            newNode = self.get_new_node(theta, n_ind, nearestNode)
            # 检查是否有障碍物 传入参数为新生城路径的两个节点
            noCollision = self.check_segment_collision(newNode.x, newNode.y, nearestNode.x, nearestNode.y)

        先看sample函数：

    def sample(self):
        # 取得1-100的随机数,如果比10大的话（以10%的概率取到终点）
        if random.randint(0, 100) > self.goal_sample_rate:
            # 在空间里随机采样一个点
            rnd = [random.uniform(self.min_rand, self.max_rand), random.uniform(self.min_rand, self.max_rand)]
        else:  # goal point sampling
            # 终点作为采样点
            rnd = [self.goal.x, self.goal.y]
        return rnd

        先取得了一个1-100的随机数，如果大于self.goal_sample_rate（传进来的参数=10），我们就启动if语句：

        它使用了 Python 中的 random.uniform() 函数来生成两个在指定范围内的随机数，并将它们放入列表 rnd 中。（-2  --> 18）

• random.uniform(a, b) 函数会返回一个在 a 和 b 之间的随机浮点数。在这里，self.min_rand 和 self.max_rand 可能是两个指定的最小值和最大值。

        所以，rnd 是一个包含两个随机数的列表，这两个随机数分别位于 self.min_rand =2和 self.max_rand  = 18之间。这个坐标作为我们随机采样的点。

        如果是else语句的话我们直接将终点作为采样点。

        因此rnd获得了如下图的红圈所示的Xrand坐标。

        在来看get_nearest_list_index函数：

    def get_nearest_list_index(nodes, rnd):
        # 遍历所有节点 计算采样点和节点的距离
        dList = [(node.x - rnd[0]) ** 2
                 + (node.y - rnd[1]) ** 2 for node in nodes]
        # 获得最近的距离所对应的索引
        minIndex = dList.index(min(dList))
        return minIndex

        第一行代码创建了一个列表 dList，其中包含了所有节点与指定点 rnd 之间的欧几里得距离的平方。

        具体来说，它使用了列表推导式（list comprehension）的语法，对于 nodes 中的每一个节点 node，计算了以下值：

(node.x - rnd[0]) ** 2 + (node.y - rnd[1]) ** 2

        最终，dList 中包含了所有节点与 rnd 之间距离的平方值。

        minIndex = dList.index(min(dList)) 获得了最近的距离所对应的索引。

        因此，nearestNode = self.node_list[n_ind]这个代码就得到了距离采样点Xrand最近的节点Xnear，如下图所示：

         theta = math.atan2(rnd[1] - nearestNode.y, rnd[0] - nearestNode.x)                    

        这行代码求了一个角度，采样点Xrand与Xnear的角度。

        我们来看get_new_node函数：

    def get_new_node(self, theta, n_ind, nearestNode):
        newNode = copy.deepcopy(nearestNode)
 
        # 坐标
        newNode.x += self.expand_dis * math.cos(theta)
        newNode.y += self.expand_dis * math.sin(theta)
        # 代价
        newNode.cost += self.expand_dis
        # 父亲节点
        newNode.parent = n_ind
        return newNode

        我们先把随机采样节点Xrand的最近节点Xnear做了深拷贝，利用三角函数计算出新的节点的坐标（1.我们传进来的参数expand_dis意为每一次的导航步长 2.expand_dis * costheta就是x的增量，y同理），因此，我们实例化了一个新节点，它的代价就是它邻近节点的代价 + expand_dis（2），它的父亲节点为这个邻近节点Xnear保证了递归的顺利进行。

        因此，

newNode = self.get_new_node(theta, n_ind, nearestNode)

       这行代码我们利用expand_dis和邻近节点Xnear新生成了一个节点。

        在来看check_segment_collision函数：

    def check_segment_collision(self, x1, y1, x2, y2):
        # 遍历所有的障碍物
        for (ox, oy, size) in self.obstacle_list:
            dd = self.distance_squared_point_to_segment(
                np.array([x1, y1]),
                np.array([x2, y2]),
                np.array([ox, oy]))
            if dd <= size ** 2:
                return False  # collision
        return True

        这个就是具体问题具体分析了，判断两条线间是否有障碍物，即Xnear和Xnew间：

        随后就是最后一部分代码啦~加加油：

            if noCollision:
                # 没有碰撞把新节点加入到树里面
                self.node_list.append(newNode)
                if animation:
                    self.draw_graph(newNode, path)
 
                # 是否到终点附近
                if self.is_near_goal(newNode):
                    # 是否这条路径与障碍物发生碰撞
                    if self.check_segment_collision(newNode.x, newNode.y,
                                                    self.goal.x, self.goal.y):
                        lastIndex = len(self.node_list) - 1
                        # 找路径
                        path = self.get_final_course(lastIndex)
                        pathLen = self.get_path_len(path)
                        print("current path length: {}, It costs {} s".format(pathLen, time.time()-start_time))
 
                        if animation:
                            self.draw_graph(newNode, path)
                        return path

        如果没有碰撞的话，我们认为这个Xnew是靠谱的，将它加入到节点队列中。再判断它是否在终点附近，我们来看is_near_goal函数：

    def is_near_goal(self, node):
        # 计算距离
        d = self.line_cost(node, self.goal)
        if d < self.expand_dis:
            return True
        return False

        这里就是计算我们新加的节点到终点的距离是否小于一次的步长2，如果小于的话就return true。

        若在终点附近，我们将这个节点和终点相连看是否中间有障碍物存在：

self.check_segment_collision(newNode.x, newNode.y,
                                                    self.goal.x, self.goal.y)

        如果没有障碍物的话，我们导航成功，开始输出路径：

    def get_final_course(self, lastIndex):
        path = [[self.goal.x, self.goal.y]]
        while self.node_list[lastIndex].parent is not None:
            node = self.node_list[lastIndex]
            path.append([node.x, node.y])
            lastIndex = node.parent
        path.append([self.start.x, self.start.y])
        return path

        我们一个一个找终止节点的父节点就可以了。

        通关～。

3 RRT完整代码

import copy
import math
import random
import time
 
import matplotlib.pyplot as plt
from scipy.spatial.transform import Rotation as Rot
import numpy as np
 
show_animation = True
 
 
class RRT:
 
    # randArea采样范围[-2--18] obstacleList设置的障碍物 maxIter最大迭代次数 expandDis采样步长为2.0 goalSampleRate 以10%的概率将终点作为采样点
    def __init__(self, obstacleList, randArea,
                 expandDis=2.0, goalSampleRate=10, maxIter=200):
 
        self.start = None
        self.goal = None
        self.min_rand = randArea[0]
        self.max_rand = randArea[1]
        self.expand_dis = expandDis
        self.goal_sample_rate = goalSampleRate
        self.max_iter = maxIter
        self.obstacle_list = obstacleList
        # 存储RRT树
        self.node_list = None
 
    # start、goal 起点终点坐标
    def rrt_planning(self, start, goal, animation=True):
        start_time = time.time()
        self.start = Node(start[0], start[1])
        self.goal = Node(goal[0], goal[1])
        # 将起点加入node_list作为树的根结点
        self.node_list = [self.start]
        path = None
 
        for i in range(self.max_iter):
            # 进行采样
            rnd = self.sample()
            # 取的距离采样点最近的节点下标
            n_ind = self.get_nearest_list_index(self.node_list, rnd)
            # 得到最近节点
            nearestNode = self.node_list[n_ind]
 
            # 将Xrandom和Xnear连线方向作为生长方向
            # math.atan2() 函数接受两个参数，分别是 y 坐标差值和 x 坐标差值。它返回的值是以弧度表示的角度，范围在 -π 到 π 之间。这个角度表示了从 nearestNode 指向 rnd 的方向。
            theta = math.atan2(rnd[1] - nearestNode.y, rnd[0] - nearestNode.x)
            # 生长 ： 输入参数为角度、下标、nodelist中最近的节点  得到生长过后的节点
            newNode = self.get_new_node(theta, n_ind, nearestNode)
            # 检查是否有障碍物 传入参数为新生城路径的两个节点
            noCollision = self.check_segment_collision(newNode.x, newNode.y, nearestNode.x, nearestNode.y)
            if noCollision:
                # 没有碰撞把新节点加入到树里面
                self.node_list.append(newNode)
                if animation:
                    self.draw_graph(newNode, path)
 
                # 是否到终点附近
                if self.is_near_goal(newNode):
                    # 是否这条路径与障碍物发生碰撞
                    if self.check_segment_collision(newNode.x, newNode.y,
                                                    self.goal.x, self.goal.y):
                        lastIndex = len(self.node_list) - 1
                        # 找路径
                        path = self.get_final_course(lastIndex)
                        pathLen = self.get_path_len(path)
                        print("current path length: {}, It costs {} s".format(pathLen, time.time()-start_time))
 
                        if animation:
                            self.draw_graph(newNode, path)
                        return path
 
    def rrt_star_planning(self, start, goal, animation=True):
        start_time = time.time()
        self.start = Node(start[0], start[1])
        self.goal = Node(goal[0], goal[1])
        self.node_list = [self.start]
        path = None
        lastPathLength = float('inf')
 
        for i in range(self.max_iter):
            rnd = self.sample()
            n_ind = self.get_nearest_list_index(self.node_list, rnd)
            nearestNode = self.node_list[n_ind]
 
            # steer
            theta = math.atan2(rnd[1] - nearestNode.y, rnd[0] - nearestNode.x)
            newNode = self.get_new_node(theta, n_ind, nearestNode)
 
            noCollision = self.check_segment_collision(newNode.x, newNode.y, nearestNode.x, nearestNode.y)
            if noCollision:
                nearInds = self.find_near_nodes(newNode)
                newNode = self.choose_parent(newNode, nearInds)
 
                self.node_list.append(newNode)
                self.rewire(newNode, nearInds)
 
                if animation:
                    self.draw_graph(newNode, path)
 
                if self.is_near_goal(newNode):
                    if self.check_segment_collision(newNode.x, newNode.y,
                                                    self.goal.x, self.goal.y):
                        lastIndex = len(self.node_list) - 1
 
                        tempPath = self.get_final_course(lastIndex)
                        tempPathLen = self.get_path_len(tempPath)
                        if lastPathLength > tempPathLen:
                            path = tempPath
                            lastPathLength = tempPathLen
                            print("current path length: {}, It costs {} s".format(tempPathLen, time.time()-start_time))
 
        return path
 
    def informed_rrt_star_planning(self, start, goal, animation=True):
        start_time = time.time()
        self.start = Node(start[0], start[1])
        self.goal = Node(goal[0], goal[1])
        self.node_list = [self.start]
        # max length we expect to find in our 'informed' sample space,
        # starts as infinite
        cBest = float('inf')
        path = None
 
        # Computing the sampling space
        cMin = math.sqrt(pow(self.start.x - self.goal.x, 2)
                         + pow(self.start.y - self.goal.y, 2))
        xCenter = np.array([[(self.start.x + self.goal.x) / 2.0],
                            [(self.start.y + self.goal.y) / 2.0], [0]])
        a1 = np.array([[(self.goal.x - self.start.x) / cMin],
                       [(self.goal.y - self.start.y) / cMin], [0]])
 
        e_theta = math.atan2(a1[1], a1[0])
 
        # 论文方法求旋转矩阵（2选1）
        # first column of identity matrix transposed
        # id1_t = np.array([1.0, 0.0, 0.0]).reshape(1, 3)
        # M = a1 @ id1_t
        # U, S, Vh = np.linalg.svd(M, True, True)
        # C = np.dot(np.dot(U, np.diag(
        #     [1.0, 1.0, np.linalg.det(U) * np.linalg.det(np.transpose(Vh))])),
        #            Vh)
 
        # 直接用二维平面上的公式（2选1）
        C = np.array([[math.cos(e_theta), -math.sin(e_theta), 0],
                      [math.sin(e_theta), math.cos(e_theta),  0],
                      [0,                 0,                  1]])
 
        for i in range(self.max_iter):
            # Sample space is defined by cBest
            # cMin is the minimum distance between the start point and the goal
            # xCenter is the midpoint between the start and the goal
            # cBest changes when a new path is found
 
            rnd = self.informed_sample(cBest, cMin, xCenter, C)
            n_ind = self.get_nearest_list_index(self.node_list, rnd)
            nearestNode = self.node_list[n_ind]
 
            # steer
            theta = math.atan2(rnd[1] - nearestNode.y, rnd[0] - nearestNode.x)
            newNode = self.get_new_node(theta, n_ind, nearestNode)
 
            noCollision = self.check_segment_collision(newNode.x, newNode.y, nearestNode.x, nearestNode.y)
            if noCollision:
                nearInds = self.find_near_nodes(newNode)
                newNode = self.choose_parent(newNode, nearInds)
 
                self.node_list.append(newNode)
                self.rewire(newNode, nearInds)
 
                if self.is_near_goal(newNode):
                    if self.check_segment_collision(newNode.x, newNode.y,
                                                    self.goal.x, self.goal.y):
                        lastIndex = len(self.node_list) - 1
                        tempPath = self.get_final_course(lastIndex)
                        tempPathLen = self.get_path_len(tempPath)
                        if tempPathLen < cBest:
                            path = tempPath
                            cBest = tempPathLen
                            print("current path length: {}, It costs {} s".format(tempPathLen, time.time()-start_time))
            if animation:
                self.draw_graph_informed_RRTStar(xCenter=xCenter,
                                                cBest=cBest, cMin=cMin,
                                                e_theta=e_theta, rnd=rnd, path=path)
 
        return path
 
    def sample(self):
        # 取得1-100的随机数,如果比10大的话（以10%的概率取到终点）
        if random.randint(0, 100) > self.goal_sample_rate:
            # 在空间里随机采样一个点
            rnd = [random.uniform(self.min_rand, self.max_rand), random.uniform(self.min_rand, self.max_rand)]
        else:  # goal point sampling
            # 终点作为采样点
            rnd = [self.goal.x, self.goal.y]
        return rnd
 
    def choose_parent(self, newNode, nearInds):
        if len(nearInds) == 0:
            return newNode
 
        dList = []
        for i in nearInds:
            dx = newNode.x - self.node_list[i].x
            dy = newNode.y - self.node_list[i].y
            d = math.hypot(dx, dy)
            theta = math.atan2(dy, dx)
            if self.check_collision(self.node_list[i], theta, d):
                dList.append(self.node_list[i].cost + d)
            else:
                dList.append(float('inf'))
 
        minCost = min(dList)
        minInd = nearInds[dList.index(minCost)]
 
        if minCost == float('inf'):
            print("min cost is inf")
            return newNode
 
        newNode.cost = minCost
        newNode.parent = minInd
 
        return newNode
 
    def find_near_nodes(self, newNode):
        n_node = len(self.node_list)
        r = 50.0 * math.sqrt((math.log(n_node) / n_node))
        d_list = [(node.x - newNode.x) ** 2 + (node.y - newNode.y) ** 2
                  for node in self.node_list]
        near_inds = [d_list.index(i) for i in d_list if i <= r ** 2]
        return near_inds
 
    def informed_sample(self, cMax, cMin, xCenter, C):
        if cMax < float('inf'):
            r = [cMax / 2.0,
                 math.sqrt(cMax ** 2 - cMin ** 2) / 2.0,
                 math.sqrt(cMax ** 2 - cMin ** 2) / 2.0]
            L = np.diag(r)
            xBall = self.sample_unit_ball()
            rnd = np.dot(np.dot(C, L), xBall) + xCenter
            rnd = [rnd[(0, 0)], rnd[(1, 0)]]
        else:
            rnd = self.sample()
 
        return rnd
 
    @staticmethod
    def sample_unit_ball():
        a = random.random()
        b = random.random()
 
        if b < a:
            a, b = b, a
 
        sample = (b * math.cos(2 * math.pi * a / b),
                  b * math.sin(2 * math.pi * a / b))
        return np.array([[sample[0]], [sample[1]], [0]])
 
    @staticmethod
    def get_path_len(path):
        pathLen = 0
        for i in range(1, len(path)):
            node1_x = path[i][0]
            node1_y = path[i][1]
            node2_x = path[i - 1][0]
            node2_y = path[i - 1][1]
            pathLen += math.sqrt((node1_x - node2_x)
                                 ** 2 + (node1_y - node2_y) ** 2)
 
        return pathLen
 
    @staticmethod
    def line_cost(node1, node2):
        return math.sqrt((node1.x - node2.x) ** 2 + (node1.y - node2.y) ** 2)
 
    @staticmethod
    def get_nearest_list_index(nodes, rnd):
        # 遍历所有节点 计算采样点和节点的距离
        dList = [(node.x - rnd[0]) ** 2
                 + (node.y - rnd[1]) ** 2 for node in nodes]
        # 获得最近的距离所对应的索引
        minIndex = dList.index(min(dList))
        return minIndex
 
    def get_new_node(self, theta, n_ind, nearestNode):
        newNode = copy.deepcopy(nearestNode)
 
        # 坐标
        newNode.x += self.expand_dis * math.cos(theta)
        newNode.y += self.expand_dis * math.sin(theta)
        # 代价
        newNode.cost += self.expand_dis
        # 父亲节点
        newNode.parent = n_ind
        return newNode
 
    def is_near_goal(self, node):
        # 计算距离
        d = self.line_cost(node, self.goal)
        if d < self.expand_dis:
            return True
        return False
 
    def rewire(self, newNode, nearInds):
        n_node = len(self.node_list)
        for i in nearInds:
            nearNode = self.node_list[i]
 
            d = math.sqrt((nearNode.x - newNode.x) ** 2
                          + (nearNode.y - newNode.y) ** 2)
 
            s_cost = newNode.cost + d
 
            if nearNode.cost > s_cost:
                theta = math.atan2(newNode.y - nearNode.y,
                                   newNode.x - nearNode.x)
                if self.check_collision(nearNode, theta, d):
                    nearNode.parent = n_node - 1
                    nearNode.cost = s_cost
 
    @staticmethod
    def distance_squared_point_to_segment(v, w, p):
        # Return minimum distance between line segment vw and point p
        if np.array_equal(v, w):
            return (p - v).dot(p - v)  # v == w case
        l2 = (w - v).dot(w - v)  # i.e. |w-v|^2 -  avoid a sqrt
        # Consider the line extending the segment,
        # parameterized as v + t (w - v).
        # We find projection of point p onto the line.
        # It falls where t = [(p-v) . (w-v)] / |w-v|^2
        # We clamp t from [0,1] to handle points outside the segment vw.
        t = max(0, min(1, (p - v).dot(w - v) / l2))
        projection = v + t * (w - v)  # Projection falls on the segment
        return (p - projection).dot(p - projection)
 
    def check_segment_collision(self, x1, y1, x2, y2):
        # 遍历所有的障碍物
        for (ox, oy, size) in self.obstacle_list:
            dd = self.distance_squared_point_to_segment(
                np.array([x1, y1]),
                np.array([x2, y2]),
                np.array([ox, oy]))
            if dd <= size ** 2:
                return False  # collision
        return True
 
    def check_collision(self, nearNode, theta, d):
        tmpNode = copy.deepcopy(nearNode)
        end_x = tmpNode.x + math.cos(theta) * d
        end_y = tmpNode.y + math.sin(theta) * d
        return self.check_segment_collision(tmpNode.x, tmpNode.y, end_x, end_y)
 
    def get_final_course(self, lastIndex):
        path = [[self.goal.x, self.goal.y]]
        while self.node_list[lastIndex].parent is not None:
            node = self.node_list[lastIndex]
            path.append([node.x, node.y])
            lastIndex = node.parent
        path.append([self.start.x, self.start.y])
        return path
 
    def draw_graph_informed_RRTStar(self, xCenter=None, cBest=None, cMin=None, e_theta=None, rnd=None, path=None):
        plt.clf()
        # for stopping simulation with the esc key.
        plt.gcf().canvas.mpl_connect(
            'key_release_event',
            lambda event: [exit(0) if event.key == 'escape' else None])
        if rnd is not None:
            plt.plot(rnd[0], rnd[1], "^k")
            if cBest != float('inf'):
                self.plot_ellipse(xCenter, cBest, cMin, e_theta)
 
        for node in self.node_list:
            if node.parent is not None:
                if node.x or node.y is not None:
                    plt.plot([node.x, self.node_list[node.parent].x], [
                        node.y, self.node_list[node.parent].y], "-g")
 
        for (ox, oy, size) in self.obstacle_list:
            plt.plot(ox, oy, "ok", ms=30 * size)
 
        if path is not None:
            plt.plot([x for (x, y) in path], [y for (x, y) in path], '-r')
 
        plt.plot(self.start.x, self.start.y, "xr")
        plt.plot(self.goal.x, self.goal.y, "xr")
        plt.axis([-2, 18, -2, 15])
        plt.grid(True)
        plt.pause(0.01)
 
    @staticmethod
    def plot_ellipse(xCenter, cBest, cMin, e_theta):  # pragma: no cover
 
        a = math.sqrt(cBest ** 2 - cMin ** 2) / 2.0
        b = cBest / 2.0
        angle = math.pi / 2.0 - e_theta
        cx = xCenter[0]
        cy = xCenter[1]
        t = np.arange(0, 2 * math.pi + 0.1, 0.1)
        x = [a * math.cos(it) for it in t]
        y = [b * math.sin(it) for it in t]
        rot = Rot.from_euler('z', -angle).as_matrix()[0:2, 0:2]
        fx = rot @ np.array([x, y])
        px = np.array(fx[0, :] + cx).flatten()
        py = np.array(fx[1, :] + cy).flatten()
        plt.plot(cx, cy, "xc")
        plt.plot(px, py, "--c")
 
    def draw_graph(self, rnd=None, path=None):
        plt.clf()
        # for stopping simulation with the esc key.
        plt.gcf().canvas.mpl_connect(
            'key_release_event',
            lambda event: [exit(0) if event.key == 'escape' else None])
        if rnd is not None:
            plt.plot(rnd.x, rnd.y, "^k")
 
        for node in self.node_list:
            if node.parent is not None:
                if node.x or node.y is not None:
                    plt.plot([node.x, self.node_list[node.parent].x], [
                        node.y, self.node_list[node.parent].y], "-g")
 
        for (ox, oy, size) in self.obstacle_list:
            # self.plot_circle(ox, oy, size)
            plt.plot(ox, oy, "ok", ms=30 * size)
 
        plt.plot(self.start.x, self.start.y, "xr")
        plt.plot(self.goal.x, self.goal.y, "xr")
 
        if path is not None:
            plt.plot([x for (x, y) in path], [y for (x, y) in path], '-r')
 
        plt.axis([-2, 18, -2, 15])
        plt.grid(True)
        plt.pause(0.01)
 
 
class Node:
 
    def __init__(self, x, y):
        self.x = x
        self.y = y
        self.cost = 0.0
        self.parent = None
 
 
def main():
    print("Start rrt planning")
 
    # create obstacles
    # obstacleList = [
    #     (3,  3,  1.5),
    #     (12, 2,  3),
    #     (3,  9,  2),
    #     (9,  11, 2),
    # ]
 
    # 设置障碍物 (圆点、半径)
    obstacleList = [(5, 5, 1), (3, 6, 2), (3, 8, 2), (3, 10, 2), (7, 5, 2),
                    (9, 5, 2), (8, 10, 1)]
 
    # Set params
    # 采样范围 设置的障碍物 最大迭代次数
    rrt = RRT(randArea=[-2, 18], obstacleList=obstacleList, maxIter=200)
    # 传入的是起点和终点
    path = rrt.rrt_planning(start=[0, 0], goal=[15, 12], animation=show_animation)
    # path = rrt.rrt_star_planning(start=[0, 0], goal=[15, 12], animation=show_animation)
    # path = rrt.informed_rrt_star_planning(start=[0, 0], goal=[15, 12], animation=show_animation)
    print("Done!!")
 
    if show_animation and path:
        plt.show()
 
 
if __name__ == '__main__':
    main()