1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
| def Planning(self):
'''
RRT_CONNECT planning
'''
'''
# 初始化两颗树：分别在起点qstart和终点qgoal构造两棵树,
# 节点树分别记为vertices_t1和vetices_t2.
'''
vertices_t1 = []
self.AddVertices(self.qstart, vertices_t1)
self.AddEdges(None, self.qstart)
vertices_t2 = []
self.AddVertices(self.qgoal, vertices_t2)
self.AddEdges(None, self.qgoal)
'''
# 开始迭代
'''
k = 0
new_qgoal = self.qgoal
while k <= self.max_steps:
k += 1
'''
# 随机采样qrand_t1作为目标点，沿(qnear_t1, qrand_t1)方向,
# 以step_size步长扩展第一棵树的节点qnew_t1.
'''
qrand_t1 = self.GenerateRandomNode(
[0, self.map_shape[0]], [0, self.map_shape[1]],
new_qgoal, self.goal_prob)
_, qnear_t1 = self.FindNearestNode(qrand_t1, vertices_t1)
qnew_t1 = self.ExtendTree(qnear_t1, qrand_t1, self.step_size)
'''
# 检查qnew_t1是否扩展成功且没有碰撞
'''
if qnew_t1 and self.CollsionFree(qnear_t1, qnew_t1, self.grid_size):
'''
将qnew_t1加入到第一颗树vertices_t1中
'''
self.AddVertices(qnew_t1, vertices_t1)
self.AddEdges(qnear_t1, qnew_t1)
self.DrawEdges(self.src_map, qnear_t1, qnew_t1)
'''
# 以qnew_t1为目标点，沿(qnear_t2, qnew_t1)方向
# 以step_size步长扩展第二棵树的节点qnew_t2.
'''
k += 1
_, qnear_t2 = self.FindNearestNode(qnew_t1, vertices_t2)
qnew_t2 = self.ExtendTree(qnear_t2, qnew_t1, self.step_size)
'''
# 检查qnew_t1是否扩展成功且没有碰撞
'''
if qnew_t2 and self.CollsionFree(qnear_t2, qnew_t2, self.grid_size):
'''
# 将qnew_t1加入到第一颗树vertices_t1中
'''
self.AddVertices(qnew_t2, vertices_t2)
self.AddEdges(qnear_t2, qnew_t2)
self.DrawEdges(self.src_map, qnear_t2, qnew_t2)
'''
# 这里一直以qnew_t1为目标点，扩展qnew_t2，
# 除非qnew_t2扩展失败或qnew_t2到达qnew_t1。
# 扩展qnew_t2带有一定的贪婪特性，所以扩展树相比RRT更快速
'''
while k <= self.max_steps and self.IsArrival(qnew_t2, qnew_t1, self.step_size) == False:
k += 1
'''
# 以qnew_t1为目标点，沿(qnew_t2, qnew_t1)方向
# 以step_size步长扩展新节点qnew_mid.
'''
qnew_mid = self.ExtendTree(
qnew_t2, qnew_t1, self.step_size)
'''
# 检查qnew_mid是否扩展成功且没有碰撞
'''
if qnew_mid and self.CollsionFree(qnew_t2, qnew_mid, self.grid_size):
'''
# 如果qnew_mid扩展成功，则将其加入到第二棵树中.
'''
self.AddVertices(qnew_mid, vertices_t2)
self.AddEdges(qnew_t2, qnew_mid)
self.DrawEdges(self.src_map, qnew_t2, qnew_mid)
'''
# 将第二棵树新扩展的节点qnew_mid作为qnew_t2继续扩展
'''
qnew_t2 = qnew_mid
else:
'''
# 如果qnew_mid扩展失败，则跳出该循环.
'''
break
'''
检查两棵树是否相连,若相连，则完成路径规划
'''
if self.IsArrival(qnew_t2, qnew_t1, self.step_size):
print("Found")
'''
# 如果两棵树连上了，说明找到路径了，把最后一个节点添加到树中。
'''
copy_qnew_t1 = copy.deepcopy(qnew_t1)
self.AddVertices(copy_qnew_t1, vertices_t2)
self.AddEdges(qnew_t2, copy_qnew_t1)
'''
# 合并两棵树，注意终点树的节点指针与起点树反向
'''
path = self.MergeTree(vertices_t1, vertices_t2)
self.DrawPath(self.src_map, path)
'''
# 路径圆滑
'''
smooth_path = self.SmoothPath(path, self.grid_size)
self.DrawPath(self.src_map, smooth_path)
return True
'''
# 当第二棵树扩展失败后，回到第一棵树扩展时，检查两棵树的节点个数，
# 选择较短树进行扩展，使两棵树保持相对平衡。新的目标点为较长树的最后一个节点
'''
if (len(vertices_t2) < len(vertices_t1)):
vertices_t2, vertices_t1 = vertices_t1, vertices_t2
new_qgoal = vertices_t2[-1]
print("NotFound")
return False
|
