（7-4-03）RRT算法：基于Gazebo仿真的路径规划系统(3)

（6）函数select_branch实现了RRT_*_FND算法中的选择分支策略，用于删除不再位于路径上的节点及其子节点。它接收当前达到的节点以及先前的路径作为输入，并根据路径更新图中的节点和边。随着节点的移除，函数会实时显示图的变化。最后，它返回更新后的路径。

def select_branch(G: Graph, current_node: int, path: list, map: Map) -> list:
    """
    RRT_*_FND算法中使用的选择分支算法。删除所有不再位于路径上的节点及其子节点。
    :param G: 图
    :param current_node: 已到达的节点，将删除其父节点及其子节点
    :param path: 先前的路径
    :param map: 地图
    :return: 新路径
    """
    G.start = G.vertices[current_node]
    parent = G.parent[current_node]
    G.parent[current_node] = None  # 将当前节点的父节点设置为None
    del G.children[parent][
        G.children[parent].index(current_node)]  # 从先前父节点的子节点中删除当前节点

    plt.figure()
    plot_graph(G, map.obstacles_c)
    plt.show()

    new_path = path[: path.index(current_node) + 1]
    stranded_nodes = path[path.index(current_node) + 1:]
    for stranded_node in reversed(stranded_nodes):
        if stranded_node == current_node: break
        remove_children(G, stranded_node, path)
        plt.figure()
        plot_graph(G, map.obstacles_c)
        plt.show()

    for node in stranded_nodes:
        G.remove_vertex(node)

    return new_path

（7）函数remove_children用于从图中删除指定节点的所有子节点，条件是这些子节点不在当前路径上。

def remove_children(G: Graph, id_node: int, path: list) -> None:
    """
    如果子节点不在路径上，则删除指定节点的所有子节点。
    :param G: 图
    :param id_node: 要删除其子节点的节点的ID
    :param path: 当前路径
    :return: 无
    """
    nodes_children = G.children[id_node].copy()
    for id_child in nodes_children:
        if id_child in path: continue
        if len(G.children[id_child]) != 0:
            remove_children(G, id_child, path)
        G.remove_vertex(id_child)

（8）函数valid_path用于验证路径的有效性，并在路径中发现与障碍物碰撞的节点时，删除这些节点及其子节点，然后返回新分离的树的根节点的ID。

def valid_path(G: Graph, path: list, map: Map, previous_root: int) -> int:
    """
    ValidPath算法，用于删除与障碍物碰撞的节点以及这些节点的子节点
    :param G: 图(Graph)
    :param path: 先前的路径
    :param map: 地图(Map)
    :param previous_root: 先前树的根节点的ID
    :return: 新分离树的根节点的ID
    """
    id_separate_root = previous_root
    nodes_in_path = [(id_node, G.vertices[id_node]) for id_node in path]
    for id_node, pos_node in reversed(nodes_in_path):
        if map.is_occupied_c(pos_node):  # 如果路径中的节点与障碍物相撞，则删除它
            remove_children(G, id_node, path)
            id_remaining_child = G.children[id_node][0]  # 此节点仅剩的子节点在路径中
            G.remove_vertex(id_node)  # 删除与障碍物相撞的节点
            G.parent[id_remaining_child] = None  # 将分离根节点的父节点设为None
            id_separate_root = id_remaining_child

            plt.pause(0.001)
            plt.clf()
            plot_graph(G, map.obstacles_c)

    return id_separate_root

（9）函数reconnect实现了重新连接算法，用于在给定的图中尝试建立两个部分之间的连接。它搜索分离树的根节点附近的原始树节点，并尝试通过路径中不存在障碍物的直线连接它们。如果成功建立了连接，则返回新的路径及其成本。

def reconnect(G: Graph, path: list, map: Map, step_size: float, id_root: int, id_separate_root: int,
              last_node: int) -> tuple:
    """
    重新连接算法，用于尝试在树的两个部分之间建立连接。
    :param G: 图(Graph)
    :param path: 当前已知路径（尽管不通向目标，因为SearchBranch和ValidPath已经执行）
    :param map: 地图(Map)
    :param step_size: 步长
    :param id_root: 原始树的根节点
    :param id_separate_root: 分离树的根节点
    :param last_node: 原始路径的最后一个节点
    :return: 路径和成本的元组
    """
    reconnected = False
    close_root_nodes = get_near_nodes(G, id_separate_root, step_size, path[-1])  # 与分离树的根节点接近的原始树的节点
    pos_separate = G.vertices[id_separate_root]
    for node in close_root_nodes:
        line = Line(G.vertices[node], pos_separate)
        if not through_obstacle(line, map.obstacles_c):
            cost = calc_distance(G.vertices[node], pos_separate)
            G.add_edge(id_separate_root, node, cost)
            reconnected = True
            break

    plt.figure()
    plot_graph(G, map.obstacles_c)
    plt.show()

    path_and_cost = None
    if reconnected:
        path_and_cost = find_path(G, last_node, id_root)
        plot_graph(G, map.obstacles_c)
        plot_path(G, path_and_cost[0], "重新连接后", path_and_cost[1])

    return path_and_cost

（10）函数check_for_tree_associativity用于检查节点是否属于以给定根节点为根的树，通过沿着节点的父节点链逐步向上检查，直到到达根节点或者没有父节点时停止。如果最终节点等于给定的根节点，则返回True，表示节点属于该树。

def check_for_tree_associativity(G: Graph, root_node: int, node_to_check: int) -> bool:
    """
    检查节点是否属于以 root_node 为根的树
    :param G: 图(Graph)
    :param root_node: 树的根节点
    :param node_to_check: 将要检查的节点
    :return: 如果节点属于树，则返回 True
    """
    node = node_to_check
    parent = G.parent[node]
    while parent is not None:
        node = G.parent[node]
        parent = G.parent[node]
    return node == root_node

（11）函数check_for_tree_associativity用于检查指定节点是否属于以给定根节点为根的树中，遍历从给定节点到根节点的路径，并返回布尔值，指示该节点是否属于该树。

def check_for_tree_associativity(G: Graph, root_node: int, node_to_check: int) -> bool:
    """
    检查节点是否属于以 root_node 为根的树
    :param G: 图(Graph)
    :param root_node: 树的根节点
    :param node_to_check: 将要检查的节点
    :return: 如果节点属于树，则返回 True
    """
    node = node_to_check
    parent = G.parent[node]
    while parent is not None:
        node = G.parent[node]
        parent = G.parent[node]
    return node == root_node

（12）下面的test_select_branch函数是一个测试函数，用于测试select_branch函数的功能。该函数首先创建了一个地图和一个图，然后使用RRT_star_FN算法生成了一条路径。接下来，它在生成的路径上调用了select_branch函数，并模拟了一些节点的移除和添加障碍物的情况。然后，它调用了valid_path函数，并最终通过reconnect函数重新连接了路径。

def test_select_branch():
    map_width = 200
    map_height = 200
    start = (50, 50)
    goal = (150, 150)
    NODE_RADIUS = 5
    step_length = 15
    my_map = Map((map_width, map_height), start, goal, NODE_RADIUS)
    my_map.generate_obstacles(obstacle_count=45, size=7)
    G = Graph(start, goal, map_width, map_height)
    iteration, best_path = RRT_star_FN(G, iter_num=500, map=my_map, step_length=step_length, radius=15,
                                       node_radius=NODE_RADIUS, max_nodes=30, bias=0)

    plot_graph(G, my_map.obstacles_c)
    plt.pause(0.001)

    last_node = best_path["path"][0]

    id_to_remove = list(reversed(best_path["path"]))[4]

    best_path["path"] = select_branch(G, id_to_remove, best_path["path"], my_map)

    my_map.add_obstacles([[G.vertices[best_path["path"][6]], 7]])
    my_map.add_obstacles([[G.vertices[best_path["path"][7]], 7]])

    plt.figure()
    plot_graph(G, my_map.obstacles_c)
    plt.show()

    id_separate_root = valid_path(G, best_path["path"], my_map, id_to_remove)

    print(f"RRT_star algorithm stopped at iteration number: {iteration}")
    plt.figure()
    plot_graph(G, my_map.obstacles_c)
    plt.show()

    root_node = G.id_vertex[G.start]
    ret_value = reconnect(G, best_path["path"], my_map, step_length*5, root_node, id_separate_root, last_node)
    if ret_value is None:
        regrow(G=G, map=my_map, step_length=step_length, radius=step_length, id_root=root_node,
               id_separate_root=id_separate_root, last_node=last_node, bias=0.02)

（13）函数regrow用于重新生成路径的算法，在尝试重新连接根树和分离树的过程中，根据随机节点生成新节点，并尝试将其连接到最近的节点，直到达到最大迭代次数或重新连接成功。通过可选参数调整算法的行为，例如允许的最大边长、重连算法执行的圆形区域的半径以及是否对节点生成进行偏置。

def regrow(G: Graph, map: Map, step_length: float, radius: float, id_root: int,
           id_separate_root: int, last_node: int, bias: float):
    """
    Regrow算法用于重新生成路径，试图重新连接根树和分离树。
    :param G: 图(Graph)
    :param map: 地图(Map)
    :param step_length: 允许的最大边长
    :param radius: 重连算法执行的圆形区域的半径
    :param id_root: 原始树的根节点的ID
    :param id_separate_root: 分离树的根节点的ID
    :param last_node: 原始路径的最后一个节点
    :param bias: 0-1，朝向目标节点的偏置
    :return: None
    """
    separate_tree = [node for node in G.vertices if check_for_tree_associativity(G, id_separate_root, node)]

    iter_num = 500
    iter = 0
    reconnected = False
    n_of_nodes = len(G.vertices)
    obstacles = map.obstacles_c
    while iter < iter_num and not reconnected:
        q_rand = G.random_node(bias=bias)  # 生成随机节点
        if map.is_occupied_c(q_rand): continue  # 如果生成的随机节点在障碍物上，则继续
        q_near, id_near = nearest_node(G, q_rand, obstacles, separate_tree)  # 找到距离随机节点最近的节点
        if q_near is None or q_rand == q_near: continue  # 随机节点无法连接到最近的节点而没有障碍物
        q_new = steer(q_rand, q_near, step_length)  # 获取新节点的位置
        if map.is_occupied_c(q_new): continue
        id_new = G.add_vertex(q_new)  # 获取新节点的ID
        n_of_nodes += 1
        cost_new_near = calc_distance(q_new, q_near)  # 计算从q_new到q_near的距离
        best_edge = (id_new, id_near, cost_new_near)
        G.cost[id_new] = cost_new_near  # 计算从最近节点到新节点的成本
        G.parent[id_new] = id_near

        choose_parent(G, q_new, id_new, best_edge, radius, obstacles, separate_tree)
        G.add_edge(*best_edge)

        iter += 1

        for node in separate_tree:  # 尝试重新连接根树和分离树
            line = Line(q_new, G.vertices[node])
            if through_obstacle(line, obstacles): continue
            if calc_distance(q_new, G.vertices[node]) < step_length:
                # 删除祖先节点(G, node)
                reconnect_ancestors(G, node)
                G.add_edge(node, id_new, calc_distance(q_new, G.vertices[node]))
                reconnected = True
                break

        plt.pause(0.001)
        plt.clf()
        plot_graph(G, obstacles)

    path = find_path(G, last_node, id_root)
    plt.figure()
    plot_graph(G, obstacles)
    plot_path(G, path[0], "after regrow")
    plt.show()

（14）函数 reconnect_ancestors 用于重新连接指定节点的祖先。它从给定节点开始，沿着其父节点向上遍历，逐级重新连接父节点和祖父节点，直到根节点。这样可以将断开的祖先节点重新连接到树中，确保树的完整性。

def reconnect_ancestors(G: Graph, id_node: int) -> None:
    """
    重新连接指定节点的祖先
    :param G: 图(Graph)
    :param id_node: 要重新连接祖先的节点ID
    :return: 无
    """
    node = id_node
    parent = G.parent[node]
    if parent is None:
        return
    while G.parent[parent] is not None:
        parent_parent = G.parent[parent]
        G.parent[parent] = node
        G.children[node].append(parent)
        del G.children[parent][G.children[parent].index(node)]
        node = parent
        parent = parent_parent
    G.parent[parent] = node

（15）函数 delete_ancestors 用于删除指定节点的所有祖先节点以及它们的子节点。它从给定节点开始，沿着其父节点向上遍历，逐级删除父节点和祖父节点，同时删除它们的子节点，直到根节点。

def delete_ancestors(G: Graph, id_node: int) -> None:
    """
    删除指定节点的所有祖先节点及其子节点
    :param G: 图(Graph)
    :param id_node: 要删除祖先的节点ID
    :return: 无
    """
    node = id_node
    parent = G.parent[node]
    while parent is not None:
        node = parent
        remove_children(G, node, [])
        parent = G.parent[node]

（16）函数intersection_circle用于检查直线段和圆之间是否相交。它接受一条直线和一个圆作为输入，并返回一个布尔值，指示它们是否相交。

def intersection_circle(line: Line, circle: list) -> bool:
    """
    检查直线段和圆之间是否相交。
    :param line: 直线
    :param circle: 圆
    :return: 如果相交返回True，否则返回False
    """
    p1, p2 = line.p1, line.p2
    if line.dir == float("inf"):  # 当直线为垂直线时，计算delta无效
        if abs(circle[0][0] - line.x_pos) <= circle[1]:
            return True
        else:
            return False
    delta, a, b = calc_delta(line, circle)
    if delta < 0:  # 无实数解
        return False
    ip1, ip2 = delta_solutions(delta, a, b)  # 相交点1的x坐标；相交点2的x坐标
    ip1[1] = line.calculate_y(ip1[0])
    ip2[1] = line.calculate_y(ip2[0])
    if is_between(ip1, p1, p2) or is_between(ip2, p1, p2):
        return True
    return False

（17）函数through_obstacle用于检查直线是否穿过障碍物，它接受一条直线和障碍物列表作为输入，并返回一个布尔值，指示直线是否与任何障碍物相交。

def through_obstacle(line: Line, obstacles: list) -> bool:  # 目前仅适用于圆形障碍物
    """
    检查直线是否穿过障碍物。
    :param line: 要检查的直线
    :param obstacles: 障碍物列表
    :return: 如果与障碍物相撞返回True，否则返回False
    """
    for obstacle in obstacles:
        if intersection_circle(line, obstacle):
            return True
    return False

（18）函数delta_solutions用于计算二次方程的解，根据给定的 delta 和二次方程的系数，返回两个解。

def delta_solutions(delta: float, a: float, b: float) -> tuple:
    """
    根据 delta 和二次方程的系数计算解。
    :param delta: delta
    :param a: x^2 的系数
    :param b: x 的系数
    :return: 两个解，x1 和 x2
    """
    x1 = [(-b + delta ** 0.5) / (2 * a), None]
    x2 = [(-b - delta ** 0.5) / (2 * a), None]
    return x1, x2

（19）函数 calc_delta 用于计算直线和圆之间的交点的 delta 值，以及方程的系数，并返回这些值的元组。

def calc_delta(line: Line, circle: list) -> tuple:
    """
    计算直线和圆之间的交点的 delta 值，以及方程的系数，并返回这些值的元组。
    :param line: 直线
    :param circle: 圆
    :return: delta 值，x^2 的系数，x 的系数 的元组
    """
    x0 = circle[0][0]  # 圆的中心 x 坐标
    y0 = circle[0][1]  # 圆的中心 y 坐标
    r = circle[1]  # 圆的半径
    a = (1 + line.dir ** 2)
    b = 2 * (-x0 + line.dir * line.const_term - line.dir * y0)
    c = -r ** 2 + x0 ** 2 + y0 ** 2 - 2 * y0 * line.const_term + line.const_term ** 2
    delta = b ** 2 - 4 * a * c
    return delta, a, b

（20）函数 is_between 用于检查点 pb 是否位于线段 (p1, p2) 上。

def is_between(pb, p1, p2):
    """
    检查点 pb 是否位于线段 (p1, p2) 上。
    :param pb: 要检查的点
    :param p1: 线段上的第一个点
    :param p2: 线段上的第二个点
    :return: 如果 pb 在 p1 和 p2 之间则返回 True，否则返回 False
    """
    check1 = pb[0] > min(p1[0], p2[0])
    check2 = pb[0] < max(p1[0], p2[0])

    return check1 and check2