@@ -82,13 +82,13 @@ def planning(self, sx, sy, gx, gy):
8282
8383 c_id_A = min (
8484 open_set_A ,
85- key = lambda o : self .find_lowest_cost (open_set_A , o , current_B ))
85+ key = lambda o : self .find_total_cost (open_set_A , o , current_B ))
8686
8787 current_A = open_set_A [c_id_A ]
8888
8989 c_id_B = min (
9090 open_set_B ,
91- key = lambda o : self .find_lowest_cost (open_set_B , o , current_A ))
91+ key = lambda o : self .find_total_cost (open_set_B , o , current_A ))
9292
9393 current_B = open_set_B [c_id_B ]
9494
@@ -148,7 +148,7 @@ def planning(self, sx, sy, gx, gy):
148148 if n_id_B in closed_set_B :
149149 continue_B = True
150150
151- if not ( continue_A ) :
151+ if not continue_A :
152152 if n_id_A not in open_set_A :
153153 # discovered a new node
154154 open_set_A [n_id_A ] = child_node_A
@@ -157,7 +157,7 @@ def planning(self, sx, sy, gx, gy):
157157 # This path is the best until now. record it
158158 open_set_A [n_id_A ] = child_node_A
159159
160- if not ( continue_B ) :
160+ if not continue_B :
161161 if n_id_B not in open_set_B :
162162 # discovered a new node
163163 open_set_B [n_id_B ] = child_node_B
@@ -202,10 +202,11 @@ def calc_heuristic(n1, n2):
202202 d = w * math .hypot (n1 .x - n2 .x , n1 .y - n2 .y )
203203 return d
204204
205- def find_lowest_cost (self , open_set , lambda_ , n1 ):
206- cost = open_set [lambda_ ].cost + \
207- self .calc_heuristic (n1 , open_set [lambda_ ])
208- return cost
205+ def find_total_cost (self , open_set , lambda_ , n1 ):
206+ g_cost = open_set [lambda_ ].cost
207+ h_cost = self .calc_heuristic (n1 , open_set [lambda_ ])
208+ f_cost = g_cost + h_cost
209+ return f_cost
209210
210211 def calc_grid_position (self , index , minp ):
211212 """
0 commit comments