（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