|
15 | 15 | show_animation = True
|
16 | 16 |
|
17 | 17 |
|
18 |
| -class BidirAStarPlanner: |
| 18 | +class BidirectionalAStarPlanner: |
19 | 19 |
|
20 | 20 | def __init__(self, ox, oy, reso, rr):
|
21 | 21 | """
|
@@ -125,44 +125,48 @@ def planning(self, sx, sy, gx, gy):
|
125 | 125 |
|
126 | 126 | # expand_grid search grid based on motion model
|
127 | 127 | for i, _ in enumerate(self.motion):
|
128 |
| - node = self.Node(current_A.x + self.motion[i][0], |
| 128 | + continue_A = False |
| 129 | + continue_B = False |
| 130 | + |
| 131 | + child_node_A = self.Node(current_A.x + self.motion[i][0], |
129 | 132 | current_A.y + self.motion[i][1],
|
130 | 133 | current_A.cost + self.motion[i][2], c_id_A)
|
131 |
| - n_id = self.calc_grid_index(node) |
132 | 134 |
|
133 |
| - # If the node is not safe, do nothing |
134 |
| - if not self.verify_node(node): |
135 |
| - continue |
136 |
| - |
137 |
| - if n_id in closed_set_A: |
138 |
| - continue |
139 |
| - |
140 |
| - if n_id not in open_set_A: |
141 |
| - open_set_A[n_id] = node # discovered a new node |
142 |
| - else: |
143 |
| - if open_set_A[n_id].cost > node.cost: |
144 |
| - # This path is the best until now. record it |
145 |
| - open_set_A[n_id] = node |
146 |
| - |
147 |
| - for i, _ in enumerate(self.motion): |
148 |
| - node = self.Node(current_B.x + self.motion[i][0], |
| 135 | + child_node_B = self.Node(current_B.x + self.motion[i][0], |
149 | 136 | current_B.y + self.motion[i][1],
|
150 | 137 | current_B.cost + self.motion[i][2], c_id_B)
|
151 |
| - n_id = self.calc_grid_index(node) |
152 | 138 |
|
153 |
| - # If the node is not safe, do nothing |
154 |
| - if not self.verify_node(node): |
155 |
| - continue |
156 |
| - |
157 |
| - if n_id in closed_set_B: |
158 |
| - continue |
| 139 | + n_id_A = self.calc_grid_index(child_node_A) |
| 140 | + n_id_B = self.calc_grid_index(child_node_B) |
159 | 141 |
|
160 |
| - if n_id not in open_set_B: |
161 |
| - open_set_B[n_id] = node # discovered a new node |
162 |
| - else: |
163 |
| - if open_set_B[n_id].cost > node.cost: |
164 |
| - # This path is the best until now. record it |
165 |
| - open_set_B[n_id] = node |
| 142 | + # If the node is not safe, do nothing |
| 143 | + if not self.verify_node(child_node_A): |
| 144 | + continue_A = True |
| 145 | + |
| 146 | + if not self.verify_node(child_node_B): |
| 147 | + continue_B = True |
| 148 | + |
| 149 | + if n_id_A in closed_set_A: |
| 150 | + continue_A = True |
| 151 | + |
| 152 | + if n_id_B in closed_set_B: |
| 153 | + continue_B = True |
| 154 | + |
| 155 | + if not(continue_A): |
| 156 | + if n_id_A not in open_set_A: |
| 157 | + open_set_A[n_id_A] = child_node_A # discovered a new node |
| 158 | + else: |
| 159 | + if open_set_A[n_id_A].cost > child_node_A.cost: |
| 160 | + # This path is the best until now. record it |
| 161 | + open_set_A[n_id_A] = child_node_A |
| 162 | + |
| 163 | + if not(continue_B): |
| 164 | + if n_id_B not in open_set_B: |
| 165 | + open_set_B[n_id_B] = child_node_B # discovered a new node |
| 166 | + else: |
| 167 | + if open_set_B[n_id_B].cost > child_node_B.cost: |
| 168 | + # This path is the best until now. record it |
| 169 | + open_set_B[n_id_B] = child_node_B |
166 | 170 |
|
167 | 171 | rx, ry = self.calc_final_path_bidir(meetpointA, meetpointB, closed_set_A, closed_set_B)
|
168 | 172 |
|
@@ -318,7 +322,7 @@ def main():
|
318 | 322 | plt.grid(True)
|
319 | 323 | plt.axis("equal")
|
320 | 324 |
|
321 |
| - bidir_a_star = BidirAStarPlanner(ox, oy, grid_size, robot_radius) |
| 325 | + bidir_a_star = BidirectionalAStarPlanner(ox, oy, grid_size, robot_radius) |
322 | 326 | rx, ry = bidir_a_star.planning(sx, sy, gx, gy)
|
323 | 327 |
|
324 | 328 | if show_animation: # pragma: no cover
|
|
0 commit comments