（1-6-02） Dijkstra算法：基于Pygame模拟的自动驾驶系统（2）

1.6.4  路径规划

在“MotionPlanning”目录中提供了用于实现自动驾驶汽车路径规划的功能。其中包含了两个程序文件samplingplanner.py和gridplanner.py，这两个文件共同构成了一个用于路径规划和模拟的模块，可以根据给定的环境地图和起始目标点，进行路径规划，并将规划结果可视化展示出来。同时，还提供了生成自动驾驶车辆行动序列的功能，用于实现自动驾驶车辆在给定路径上的运动控制。

1. 文件statespace.py

在文件statespace.py包含了与状态空间、地图、树结构以及节点相关的类和方法。其中类RoadMap负责定义一个道路地图，包括障碍物的设置、碰撞检测、路径采样等功能。而 类Tree和类Node则实现了树结构和节点的相关操作，包括添加边、寻找最近邻节点、计算代价等。这些功能共同构成了运动规划算法的核心组成部分，用于在给定地图上搜索可行路径。

（1）下面的代码定义了类RoadMap，其中包含了对道路地图的初始化、碰撞检测、随机采样、车道检查、矩形区域判断、创建地图、绘制多边形、绘制箭头、绘制圆形、绘制线段以及绘制散点等功能的实现。该类用于在给定的游戏环境和世界条件下构建道路地图，并提供了一系列用于地图处理和可视化的方法。

# 欧几里得距离方法
euclidean = lambda vector1, vector2: np.linalg.norm(np.array(vector1) - np.array(vector2))

# 曼哈顿距离方法
manhattan = lambda vector1, vector2: np.sum(np.absolute(np.array(vector1) - np.array(vector2)))

class RoadMap:
    def __init__(self, game, world):
        # 初始化道路地图
        self.game = game
        # 离散化空间参数
        start = (410, 408)  # 起点
        goal = (world.width_px - 1.5 * world.window.finish.get_width(), world.height_px / 2)  # 终点
        self.start, self.goal = Node(start), Node(goal)
        self.width = world.width_px
        self.height = world.height_px - 180
        self.goalRadius = 30  # 目标半径
        self.lanes_y = [180, 270, 360, 450, 540]  # 车道位置
        # 障碍物参数
        self.verticalOffset = 30  # 垂直偏移
        self.horizontalOffset = 30  # 水平偏移
        # 机器人参数
        self.robotRadius = game.orange_car.car_width_px // 2  # 机器人半径
        self.stepSize = 50  # 步长
        ## 轨迹参数
        self.mergeDistance = 2 * game.orange_car.car_width_px  # 合并距离

    def collisionAvoidance(self, state):
        # 碰撞避免
        p = state[:2]
        isNotObstacle = True
        for obstacle in self.game.obst_list:
            if self.inRectangle(p, obstacle):
                isNotObstacle = False
                break
        return isNotObstacle

    def sample(self, goalProbability):
        # 随机采样
        if random.random() > goalProbability:
            return Node((random.uniform(self.start.state[0], self.width), random.uniform(180, self.height)))
        else:
            return self.goal

    def checkLane(self, waypoint):
        # 检查车道
        lanes = self.lanes_y
        y = waypoint[1]
        if lanes[0] <= y < lanes[1]:
            return 3  # 最左侧车道
        elif lanes[1] <= y < lanes[2]:
            return 2
        elif lanes[2] <= y < lanes[3]:
            return 1
        elif lanes[3] <= y < lanes[4]:
            return 0  # 最右侧车道
        else:
            return None

    def inRectangle(self, point, obstacle):
        # 是否在矩形内
        # 机器人当前位置
        x, y = point[0], point[1]  # (列, 行)
        # 矩形参数
        x_L = obstacle.spritex - self.horizontalOffset
        x_U = obstacle.spritex + obstacle.car_width_px + self.horizontalOffset
        y_L = obstacle.spritey - self.verticalOffset
        y_U = obstacle.spritey + obstacle.car_height_px + self.verticalOffset
        return x_L <= x <= x_U and y_L <= y <= y_U

    def contain(self, state):
        # 是否包含在地图内
        return self.start.state[0] <= state[0] <= self.width and 180 <= state[1] <= self.height

    def create(self):
        # 创建地图
        figure = plt.figure(figsize=(11, 2))
        self.ax = plt.axes(xlim=(0, self.width), ylim=(180, self.height))
        self.ax.invert_yaxis()
        self.ax.set_aspect('equal')
        plt.xlabel('x')
        plt.ylabel('y')
        plt.title('Map')
        # 绘制障碍物
        for obstacle in self.game.obst_list:
            x_L = obstacle.spritex
            x_U = x_L + obstacle.car_width_px
            y_L = obstacle.spritey
            y_U = y_L + obstacle.car_height_px
            self.drawPolygon((x_L, x_U, y_L, y_U))
        # 绘制车道
        for i in range(1, 4):
            lane_y = self.lanes_y[i]
            plt.plot((0, self.width), (lane_y, lane_y), '--', color='green', linewidth=0.5)

        return figure

    def drawPolygon(self, parameters):
        # 绘制多边形
        if isinstance(parameters, tuple):
            x_L, x_U, y_L, y_U = parameters
            vertices = [(x_L, y_L), (x_U, y_L), (x_U, y_U), (x_L, y_U)]
        else:
            vertices = parameters

        polygon = Polygon(vertices, linewidth=0.5, color='blue')
        self.ax.add_artist(polygon)

    def drawArrow(self, start, end, color='black'):
        # 绘制箭头
        dx = end[0] - start[0]
        dy = end[1] - start[1]
        # arrow = plt.quiver(start[0], start[1], end[0], end[1], color=color, units = 'xy', scale = 5)
        arrow = plt.arrow(start[0], start[1], dx, dy, color=color, length_includes_head=True, head_width=0.1, head_length=0.15)
        self.ax.add_artist(arrow)

        return arrow

    def drawCircle(self, parameters, color='blue'):
        # 绘制圆形
        xc, yc, r = parameters
        circle = Circle((xc, yc), r, linewidth=0.5, color=color)
        self.ax.add_artist(circle)
        return circle

    @staticmethod
    def drawSegment(segment):
        # 绘制线段
        # p (x, y) 在图像坐标中
        X = list(map(lambda p: p[0], segment))
        Y = list(map(lambda p: p[1], segment))
        plt.plot(X, Y, '-o', color='cyan', linewidth=0.5, markersize=0.1, zorder=0)

    @staticmethod
    def scatter(X, Y):
        # 绘制散点
        plt.plot(X, Y, 'o', color='cyan', markersize=0.5, zorder=0)

（2）下面这段代码定义了一个名为 Tree 的类，用于表示搜索树。其中包含了如下所示的功能。

1. __init__：初始化搜索树，接受一个根节点作为参数，并将其存储在 nodes 列表中。
2. addEdge：向搜索树中添加一条边，连接现有节点和新节点，并将新节点的父节点指向现有节点。
3. nearestNeighbor：查找搜索树中与给定状态最近的节点，并返回该节点及其与给定状态的距离。

class Tree:
    def __init__(self, rootNode):
        self.nodes = [rootNode] # robot states 

    def addEdge(self, existingNode, newNode):
        newNode.parent = existingNode
        self.nodes.append(newNode)

    def nearestNeighbor(self, randomState):
        minDistance = np.inf
        for state in self.nodes:
            distance = state.cost2go(randomState)
            if distance < minDistance:
                minDistance = distance
                nearestNode = state

        return (nearestNode, minDistance)

（3）下面这段代码定义了一个名为 Node 的类，用于表示搜索树中的节点。该类具有如下所示的功能：

1. __init__：初始化节点，接受状态、动作和父节点作为参数，并将它们存储在相应的属性中。
2. neighbors：计算节点的相邻节点，并返回相邻节点的列表。该方法根据给定的地图 (Map) 和步长 (Map.stepSize) 计算相邻节点的位置，并检查这些节点是否在地图范围内并避开障碍物。
3. cost2go：计算从当前节点到另一个节点的代价，使用给定的启发式方法 (默认为欧几里得距离)。
4. discretize：将节点的状态离散化，返回在网格 (grid) 上的索引。
5. stoppingState：判断节点是否是停止状态。如果节点是另一个节点，则返回 None。否则，如果节点与另一个节点之间的距离小于最大分支大小 (maxBranchSize)，并且在状态空间 (stateSpace) 中避免了与另一个节点的碰撞，则返回另一个节点；否则返回 None。
6. __eq__ 和 __lt__：重载比较运算符，以便在节点之间进行比较。

class Node:
    def __init__(self, state, action=None, parent=None):
        self.state = state  # 机器人状态 (位置 + 方向) 存储为 (x, y, theta)
        self.action = action  # 从父节点执行的动作以产生此状态
        self.parent = parent  # 前一个节点
        self.trajectory = []  # 从父节点到子节点的插值状态列表

    def neighbors(self, Map):    
        adj = []
        # x, y, theta = self.state
        x, y, theta = self.state[0], self.state[1], 0
      
        # 动作集合 = {作为角度定义的移动方向}
        no_actions = 8
        d = Map.stepSize
        actions = np.linspace(np.deg2rad(-180), np.deg2rad(180), no_actions, endpoint=True) 
        for action in actions:
            angle = theta + action  # 弧度制
            if angle <= -np.pi:
                angle += 2*np.pi
            if angle > np.pi:
                angle -= 2*np.pi
            adj.append(((x + d*np.cos(angle), y + d*np.sin(angle), angle), action))

        return [(Node(neighbor[0], neighbor[1]), Map.stepSize) for neighbor in adj if Map.contain(neighbor[0]) and Map.collisionAvoidance(neighbor[0])]

    def cost2go(self, other, heuristic=euclidean):
        return heuristic(self.state[:2], other.state[:2])

    def discretize(self, grid):
        xIndex = self.index(self.state[0], grid.positionResolution)
        yIndex = self.index(self.state[1], grid.positionResolution)
        # thetaIndex = int(self.state[2]/grid.angleResolution)
        thetaIndex = 0
        return xIndex + grid.sizeX*(yIndex + thetaIndex*grid.sizeY)

    @staticmethod
    def index(num, threshold):
        # 浮点数舍入误差需要放大
        amplification = 1e6  # 更高的放大比允许更低的阈值
        num_int = num // 1
        num_dec = (num % 1)
        quotient = num_dec // threshold
        num_dec *= amplification
        threshold *= amplification
        remainder = num_dec % threshold
        if quotient:
            if remainder >= threshold/2:
                i = num_int + (quotient*threshold + threshold)/amplification
            else:
                i = num_int + quotient*threshold/amplification
        else:
            if remainder >= threshold/2: 
                i = num_int + threshold/amplification
            else:
                i = num_int
        return int(i/threshold*amplification)

    def stoppingState(self, stateSpace, other, maxBranchSize, distance):
        if self == other:
            newNode = None
        elif distance <= maxBranchSize and stateSpace.collisionAvoidance(other.state):
            newNode = other
        else:
            p1, p2 = self.state, other.state
            ratio = maxBranchSize / distance
            x_pi = p1[0] + ratio*(p2[0]-p1[0])
            y_pi = p1[1] + ratio*(p2[1]-p1[1])
            newNode = Node((x_pi, y_pi))
            if not stateSpace.collisionAvoidance(newNode.state):
                newNode = None
        return newNode
                
    def __eq__(self, other):
        return self.state == other.state

    def __lt__(self, other): 
        # 重载这个运算符是为了让 "heapq" 起作用
        return self.state < other.state

2. 文件motionplanner.py

文件motionplanner.py定义了两个路径规划器类 SamplingPlanner 和 GridPlanner，以及与路径规划相关的辅助方法。其中类SamplingPlanner实现了基于采样的路径规划算法 RRT（Rapidly-exploring Random Tree），而 类GridPlanner则实现了基于网格的路径规划算法（包括A*、Dijkstra、BFS 和 DFS）。这些规划器可以用于在给定的状态空间中找到机器人从起点到终点的最优路径，并提供了可视化方法 visualizePathFinding 来展示路径规划的过程和结果。

（1）创建类SamplingPlanner，功能是基于随机采样的快速探索随机树算法（RRT）来进行路径规划。通过迭代生成树来尝试找到从起点到终点的路径，其中包括生成随机树、计算最近节点、执行局部路径规划、测量路径成本以及模拟路径规划过程等功能。

class SamplingPlanner:
    def __init__(self, stateSpace):
        self.stateSpace = stateSpace  # 初始化状态空间
        self._planCost = 0  # 初始化计划成本为0

    def RRT(self, maxBranchSize=30, maxTreeSize=10000, goalProbability=0.05):
        ''' 快速探索随机树 '''
        solved = False  # 标记路径规划是否完成

        start, goal = self.stateSpace.start, self.stateSpace.goal  # 获取起点和终点

        # 正向搜索
        randomTree = Tree(start)  # 创建根节点为起点的树
        for _ in range(maxTreeSize):  # 迭代搜索
            sample = self.stateSpace.sample(goalProbability)  # 采样随机点
            nearest, distance = randomTree.nearestNeighbor(sample)  # 找到最近的节点
            new = nearest.stoppingState(self.stateSpace, sample, maxBranchSize, distance)  # 在采样点附近生成新节点
            if new is None:
                continue
            if self._localPlanner(nearest, new) is not None:  # 执行局部路径规划
                randomTree.addEdge(nearest, new)  # 将新节点添加到树中
                if new.cost2go(goal, heuristic=euclidean) <= self.stateSpace.goalRadius:  # 检查是否到达终点
                    solved = True  # 路径规划完成
                    break
        exploredNodes = randomTree.nodes  # 获取探索过的节点，用于路径可视化
        # 回溯
        if solved == False:
            new, _ = randomTree.nearestNeighbor(goal)
        plan = _generatePlan(new)  # 生成最终路径
        self._measureCost(plan)  # 计算路径成本
        return (solved, plan, exploredNodes, self._planCost)

    @staticmethod
    def _localPlanner(nearestNode, newNode):
        # 轨迹（直线）规划器
        return True

    def _measureCost(self, plan):
        for i in range(1, len(plan)):
            self._planCost += plan[i-1].cost2go(plan[i])  # 计算路径成本
    def simulation(self, plan, exploredNodes, plannerType='Sampling', step=1000):
        visualizePathFinding(self.stateSpace, plan, exploredNodes, plannerType, step)

在上述代码中，类SamplingPlanner主要用于实现基于随机采样的路径规划算法，并提供了相关功能来评估和可视化路径规划的结果。具体说明如下所示。

1. 初始化方法__init__(self, stateSpace)：接受状态空间作为参数，并初始化计划成本 _planCost。
2. RRT 方法：实现了基于随机采样的快速探索随机树算法（Rapidly-exploring Random Tree）。通过迭代生成树来尝试找到从起点到终点的路径。具体步骤包括：

• 创建一个随机树 randomTree，以起点 start 作为根节点。
• 在给定的最大树大小 maxTreeSize 内，进行迭代。
• 在每次迭代中，从状态空间中随机采样一个样本点 sample。
• 找到随机树中距离样本点最近的节点 nearest，计算距离 distance。
• 根据最大分支大小 maxBranchSize 和样本点距离，确定在该方向上是否可以生成新节点 new。
• 如果可以生成新节点，并且通过本地路径规划器 _localPlanner 检查通过，则将新节点添加到随机树中。
• 如果新节点可以达到目标点 goal，则成功找到路径，标记为已解决。
• 最后，返回解决状态、找到的路径、探索过的节点以及计划成本。

1. _localPlanner 方法：静态方法，用于执行局部路径规划，此处为直线轨迹规划，始终返回 True。
2. _measureCost 方法：计算给定路径的成本，将路径中相邻节点之间的代价相加，用于评估路径的质量。
3. simulation 方法：模拟路径规划过程，可视化展示路径规划的过程和结果，包括绘制探索过的节点和找到的路径。

（2）定义类GridPlanner，用于在网格状态空间中执行不同的路径搜索算法，包括A*、Dijkstra、BFS和DFS。在初始化该类的实例时接受一个状态空间对象，并具有默认的位置和角度分辨率参数。它提供了几种路径搜索方法，每种方法都会返回一个元组，其中包含了是否找到解、解路径、探索过的节点以及路径成本。此外，该类还提供了一个模拟方法，用于可视化路径规划的结果。

class GridPlanner:
    def __init__(self, stateSpace, positionResolution=30, angleResolution=45):
        # 网格参数
        self.stateSpace = stateSpace
        self.positionResolution = positionResolution  # 在]0,1]范围内的位置分辨率
        self.angleResolution = np.deg2rad(angleResolution)  # 角度分辨率
        self.sizeX = int(stateSpace.width / self.positionResolution)
        self.sizeY = int(stateSpace.height / self.positionResolution)
        self.sizeTheta = int(2 * np.pi / self.angleResolution)
        self.gridSize = self.sizeX * self.sizeY * self.sizeTheta

    def Astar(self):
        ''' 快速最优搜索 '''
        solved, exploredNodes = False, []
        visited = np.zeros((self.gridSize,), dtype=bool)
        cost = np.full((self.gridSize,), np.inf, dtype=np.float64)
        start, goal = self.stateSpace.start, self.stateSpace.goal
        # 正向搜索
        cost[start.discretize(self)] = 0
        unvisited = [(cost[start.discretize(self)], start)]  # 基于最小堆优先队列 (pq)
        while unvisited:
            minCost, current = heapq.heappop(unvisited)  # pq.remove()
            cost2come, cost2go = cost[current.discretize(self)], current.cost2go(goal, heuristic=euclidean)
            if cost2go <= self.stateSpace.goalRadius:
                solved = True
                break
            visited[current.discretize(self)] = True
            if minCost <= cost2come + cost2go:  # 避免在pq中处理节点重复
                exploredNodes.append(current)  # 用于路径搜索的可视化
                for neighbor in current.neighbors(self.stateSpace):
                    node, edgeCost = neighbor[0], neighbor[1]
                    newcost2come = cost2come + edgeCost
                    if not visited[node.discretize(self)]:
                        if newcost2come < cost[node.discretize(self)]:
                            node.parent = current
                            cost[node.discretize(self)] = newcost2come
                            newcost2come += node.cost2go(goal, heuristic=euclidean)
                            heapq.heappush(unvisited, (newcost2come, node))  # pq.insert() / pq.updatePriority()
        # 回溯
        planCost = cost[current.discretize(self)]
        plan = _generatePlan(current)
        return (solved, plan, exploredNodes, planCost)

    def Dijkstra(self):
        ''' 最优搜索 '''
        solved, exploredNodes = False, []
        visited = np.zeros((self.gridSize,), dtype=bool)
        cost = np.full((self.gridSize,), np.inf, dtype=np.float64)
        start, goal = self.stateSpace.start, self.stateSpace.goal
        # 正向搜索
        cost[start.discretize(self)] = 0
        unvisited = [(cost[start.discretize(self)], start)]  # 基于最小堆优先队列 (pq)
        while unvisited:
            minCost, current = heapq.heappop(unvisited)  # pq.remove()
            cost2come, cost2go = cost[current.discretize(self)], current.cost2go(goal, heuristic=euclidean)
            if cost2go <= self.stateSpace.goalRadius:
                solved = True
                break
            visited[current.discretize(self)] = True
            if minCost <= cost2come:  # 避免在pq中处理节点重复
                exploredNodes.append(current)  # 用于路径搜索的可视化
                for neighbor in current.neighbors(self.stateSpace):
                    node, edgeCost = neighbor[0], neighbor[1]
                    newcost2come = cost2come + edgeCost
                    if not visited[node.discretize(self)]:
                        if newcost2come < cost[node.discretize(self)]:
                            node.parent = current
                            cost[node.discretize(self)] = newcost2come
                            heapq.heappush(unvisited, (newcost2come, node))  # pq.insert() / pq.updatePriority()
        # 回溯
        planCost = cost[current.discretize(self)]
        plan = _generatePlan(current)
        return (solved, plan, exploredNodes, planCost)

    def BFS(self):
        ''' 广度优先搜索 '''
        solved, exploredNodes = False, []
        visited = np.zeros((self.gridSize,), dtype=bool)
        start, goal = self.stateSpace.start, self.stateSpace.goal
        # 正向搜索
        unvisited = deque([start])  # 队列
        visited[start.discretize(self)] = True

        while unvisited:
            current = unvisited.pop()  # 队列出队
            if current.cost2go(goal, heuristic=euclidean) <= self.stateSpace.goalRadius:
                solved = True
                break
            exploredNodes.append(current)  # 用于路径搜索的可视化

            for neighbor in current.neighbors(self.stateSpace):
                node = neighbor[0]
                if not visited[node.discretize(self)]:
                    node.parent = current
                    unvisited.appendleft(node)  # 队列入队
                    visited[node.discretize(self)] = True

        # 回溯
        plan = _generatePlan(current)
        planCost = len(plan) - 1  # 边的数量；因为BFS不考虑边的成本
        return (solved, plan, exploredNodes, planCost)

    def DFS(self):
        ''' 深度优先搜索 '''
        solved, exploredNodes = False, []
        visited = np.zeros((self.gridSize,), dtype=bool)
        start, goal = self.stateSpace.start, self.stateSpace.goal
        # 正向搜索
        unvisited = deque([start])  # 栈
        while unvisited:
            current = unvisited.pop()  # 栈出栈
            if current.cost2go(goal, heuristic=euclidean) <= self.stateSpace.goalRadius:
                solved = True
                break
            visited[current.discretize(self)] = True
            exploredNodes.append(current)  # 用于路径搜索的可视化
            for neighbor in current.neighbors(self.stateSpace):
                node = neighbor[0]
                if not visited[node.discretize(self)]:
                    node.parent = current
                    unvisited.append(node)  # 栈入栈
        # 回溯
        plan = _generatePlan(current)
        planCost = len(plan) - 1  # 边的数量；因为DFS不考虑边的成本
        return (solved, plan, exploredNodes, planCost)

    def simulation(self, plan, exploredNodes, plannerType='Grid', step=1000):
        visualizePathFinding(self.stateSpace, plan, exploredNodes, plannerType, step)   

def _generatePlan(currentNode):
    plan = deque()  # 队列
    while (currentNode.parent is not None):
        plan.appendleft(currentNode)
        currentNode = currentNode.parent
    plan.appendleft(currentNode)
    return plan

def visualizePathFinding(stateSpace, plan, exploredNodes, plannerType, step):
    robotSize_simulation = 25
    robot_start = (*stateSpace.start.state[:2], robotSize_simulation)
    robot_goal = (*stateSpace.goal.state[:2], robotSize_simulation)
    Map = stateSpace.create()
    # 绘制探索空间
    size = len(exploredNodes) - 1
    for i in range(1, size, step):
        if size - i + 1 < step:
            step = size - i + 1
        for j in range(step):
            if plannerType == 'Grid':
                x, y = exploredNodes[i+j].state[:2]
                stateSpace.scatter(x, y)
            elif plannerType == 'Sampling':
                branch = [exploredNodes[i+j].parent.state[:2], exploredNodes[i+j].state[:2]]
                stateSpace.drawSegment(branch)
        stateSpace.drawCircle(robot_start, 'red')
        stateSpace.drawCircle(robot_goal, 'red')

    # 绘制路径
    robot = stateSpace.drawCircle(robot_start, 'yellow')
    for i in range(1, len(plan)):
        robot.remove()
        consecutiveStates = (plan[i-1].state[:2], plan[i].state[:2])
        previousState, currentState = consecutiveStates
        stateSpace.drawArrow(previousState, currentState, 'black')
        robot = stateSpace.drawCircle((*currentState, robotSize_simulation), 'yellow')
    plt.show()

def actionPlanner_SDC(stateSpace, plan):
    actions = []
    mergeDistance = stateSpace.mergeDistance
    x_start = plan[0].state[0]
    previousLane = stateSpace.checkLane(plan[0].state)
    for i in range(1, len(plan)):
        waypoint = plan[i].state
        lane = stateSpace.checkLane(waypoint)
        if lane < previousLane:
            previousWaypoint = plan[i-1].state
            distance = previousWaypoint[0] - x_start
            x_start = previousWaypoint[0] + mergeDistance
            # actions.append(distance)
            # actions.append('R')
            actions.append(('R',previousWaypoint[0]))
        elif lane > previousLane:
            previousWaypoint = plan[i-1].state
            distance = previousWaypoint[0] - x_start
            x_start = previousWaypoint[0] + mergeDistance
            # actions.append(distance)
            # actions.append('L')
            actions.append(('L',previousWaypoint[0]))
        previousLane = lane
    return actions

对上述代码的具体说明如下所示。

1. __init__方法：这是GridPlanner类的构造方法，用于初始化网格规划器对象。它接受状态空间、位置分辨率和角度分辨率等参数，并设置了网格规划器的相关属性，如状态空间、位置分辨率、角度分辨率等。
2. Astar方法：实现了在网格状态空间中进行A*算法搜索的功能，使用最小堆优先队列来动态更新节点的代价，并返回是否找到解、解路径、探索过的节点以及路径代价。
3. Dijkstra方法：是网格规划器类的方法之一，用于实现Dijkstra算法搜索的功能。它使用最小堆优先队列来动态更新节点的代价，并返回是否找到解、解路径、探索过的节点以及路径代价。
4. BFS方法：实现了在网格状态空间中进行广度优先搜索的功能，它从起始节点开始逐层扩展搜索，直到找到目标节点。该方法返回是否找到解、解路径、探索过的节点以及路径代价。
5. DFS方法：是网格规划器类的方法之一，用于实现深度优先搜索的功能。它通过深度优先策略探索状态空间，并返回是否找到解、解路径、探索过的节点以及路径代价。
6. simulation方法：用于可视化路径搜索的结果，它接收解路径和探索过的节点作为参数，并根据指定的步长将探索过的空间和路径绘制出来，以进行可视化展示。
7. _generatePlan(currentNode)方法：这个方法用于生成从当前节点到起始节点的路径计划，它从当前节点开始，沿着父节点链向上回溯，直到到达起始节点，并将路径节点添加到队列中返回。
8. visualizePathFinding(stateSpace, plan, exploredNodes, plannerType, step)方法：这个方法用于可视化路径规划过程。它根据传入的路径规划结果、探索节点以及规划类型，绘制探索空间和规划路径。同时还会绘制起始点和目标点，以及路径中的箭头表示方向。
9. actionPlanner_SDC(stateSpace, plan)方法：这个方法用于生成自动驾驶车辆的行动序列，它基于规划路径和环境状态，确定车辆应该采取的动作，以便安全地沿着规划路径行驶，并避免碰撞和违反交通规则。

未完待续