Skip to content

Commit 6f358ad

Browse files
committed
shorten motion expansion loop
1 parent 89ef0a9 commit 6f358ad

File tree

1 file changed

+37
-33
lines changed

1 file changed

+37
-33
lines changed

PathPlanning/BidirectionalAStar/bidir_a_star.py renamed to PathPlanning/BidirectionalAStar/bidirectional_a_star.py

Lines changed: 37 additions & 33 deletions
Original file line numberDiff line numberDiff line change
@@ -15,7 +15,7 @@
1515
show_animation = True
1616

1717

18-
class BidirAStarPlanner:
18+
class BidirectionalAStarPlanner:
1919

2020
def __init__(self, ox, oy, reso, rr):
2121
"""
@@ -125,44 +125,48 @@ def planning(self, sx, sy, gx, gy):
125125

126126
# expand_grid search grid based on motion model
127127
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],
129132
current_A.y + self.motion[i][1],
130133
current_A.cost + self.motion[i][2], c_id_A)
131-
n_id = self.calc_grid_index(node)
132134

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],
149136
current_B.y + self.motion[i][1],
150137
current_B.cost + self.motion[i][2], c_id_B)
151-
n_id = self.calc_grid_index(node)
152138

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)
159141

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
166170

167171
rx, ry = self.calc_final_path_bidir(meetpointA, meetpointB, closed_set_A, closed_set_B)
168172

@@ -318,7 +322,7 @@ def main():
318322
plt.grid(True)
319323
plt.axis("equal")
320324

321-
bidir_a_star = BidirAStarPlanner(ox, oy, grid_size, robot_radius)
325+
bidir_a_star = BidirectionalAStarPlanner(ox, oy, grid_size, robot_radius)
322326
rx, ry = bidir_a_star.planning(sx, sy, gx, gy)
323327

324328
if show_animation: # pragma: no cover

0 commit comments

Comments
 (0)