双向RRT*轨迹规划

智能机器人课设内容，所用语言为python

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
import matplotlib

matplotlib.use('TKAgg')

import matplotlib.pyplot as plt

from matplotlib.pyplot import plot,savefig
show_animation = False

class RRT:

    def __init__(self, obstacleList, randArea, expandDis=1.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
        self.node_list = None
        self.mode_list = None

    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]
        self.mode_list = [self.goal]
        path1 = None
        path2 = None
        lastPathLength = float('inf')  # 上一次路径
        lastPathLength1 = float('inf')  # 上一次路径
        lastPathLength2 = float('inf')  # 上一次路径

        for i in range(self.max_iter):
            rnd = self.sample()  # 随机取样
            nnr = self.sample()

            n_ind = self.get_nearest_list_index(self.node_list, rnd)  # 找到离当前点最近的节点
            m_ind = self.get_nearest_list_index(self.mode_list, nnr)  # 找到离当前点最近的节点

            nearestNode = self.node_list[n_ind]
            fairNode = self.mode_list[m_ind]  # 为终点也建立最近点

            # steer
            theta = math.atan2(rnd[1] - nearestNode.y, rnd[0] - nearestNode.x)
            beta = math.atan2(nnr[1] - fairNode.y, nnr[0] - fairNode.x)  # 计算随机取样的点和最近的点之间的关系
            newNode = self.get_new_node(theta, n_ind, nearestNode)
            oldNode = self.get_new_node(beta, m_ind, fairNode)  # 起点、终点同时拓展

            noCollision = self.check_segment_collision(newNode.x, newNode.y, nearestNode.x, nearestNode.y)
            nno = self.check_segment_collision(oldNode.x, oldNode.y, fairNode.x, fairNode.y)  # 判断从终点当前点到最近的点，是否能拓展

            if noCollision:
                nearInds = self.find_near_nodes(newNode)  # 从这里开始改
                newNode = self.choose_parent(newNode, nearInds)
                self.node_list.append(newNode)
                self.rewire(newNode, nearInds)
                for node in self.mode_list:
                    d = self.line_cost(node, newNode)
                    if d < 1.0:
                        if self.check_segment_collision(node.x, node.y,
                                                        newNode.x, newNode.y):
                            judgednode = node

                            lastIndex = len(self.node_list) - 1
                            FirstIndex = len(self.mode_list) - 1

                            tempPath1 = self.get_final_course(newNode, lastIndex)
                            tempPath2 = self.get_first_course(judgednode, FirstIndex)
                            tempPathLen1 = self.get_path_len(tempPath1)
                            tempPathLen2 = self.get_path_len(tempPath2)
                            tempPathLen = tempPathLen1 + tempPathLen2+ d

                            if lastPathLength > tempPathLen:
                                path1 = tempPath1
                                lastPathLength = tempPathLen

                                if animation:
                                    self.draw_graph(judgednode, newNode, path1, path2)
                                tempPathLen = tempPathLen1 + tempPathLen2 + d

                                print("current path length: {}, It costs {} s".format(tempPathLen,
                                                                                      time.time() - start_time))
                                if time.time() - start_time >20:
                                    break


            if nno:  # 从终点出发的生成树是否接近对面已存在的点
                nearImds = self.find_near_modes(oldNode)
                oldNode = self.hoose_parent(oldNode, nearImds)

                self.mode_list.append(oldNode)
                self.remire(oldNode, nearImds)

                for node in self.node_list:
                    d = self.line_cost(node, oldNode)
                    if d < 1.0:
                        if self.check_segment_collision(node.x, node.y,
                                                        oldNode.x, oldNode.y):
                            judgenode = node
                            lastIndex = len(self.node_list) - 1
                            FirstIndex = len(self.mode_list) - 1

                            tempPath1 = self.get_final_course(judgenode, lastIndex)
                            tempPath2 = self.get_first_course(oldNode, FirstIndex)
                            tempPathLen1 = self.get_path_len(tempPath1)
                            tempPathLen2 = self.get_path_len(tempPath2)
                            tempPathLen = tempPathLen1 + tempPathLen2+ d
                            plt.plot(oldNode.x, oldNode.y, '-k')

                            if lastPathLength > tempPathLen:
                                path2 = tempPath2
                                lastPathLength = tempPathLen
                                tempPathLen = tempPathLen1 + tempPathLen2 + d
                                if animation:
                                    self.draw_graph(judgenode, oldNode, path1, path2)
                                print("current path length: {}, It costs {} s".format(tempPathLen, time.time() - start_time))


        r