@@ -82,13 +82,13 @@ def planning(self, sx, sy, gx, gy):
82
82
83
83
c_id_A = min (
84
84
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 ))
86
86
87
87
current_A = open_set_A [c_id_A ]
88
88
89
89
c_id_B = min (
90
90
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 ))
92
92
93
93
current_B = open_set_B [c_id_B ]
94
94
@@ -148,7 +148,7 @@ def planning(self, sx, sy, gx, gy):
148
148
if n_id_B in closed_set_B :
149
149
continue_B = True
150
150
151
- if not ( continue_A ) :
151
+ if not continue_A :
152
152
if n_id_A not in open_set_A :
153
153
# discovered a new node
154
154
open_set_A [n_id_A ] = child_node_A
@@ -157,7 +157,7 @@ def planning(self, sx, sy, gx, gy):
157
157
# This path is the best until now. record it
158
158
open_set_A [n_id_A ] = child_node_A
159
159
160
- if not ( continue_B ) :
160
+ if not continue_B :
161
161
if n_id_B not in open_set_B :
162
162
# discovered a new node
163
163
open_set_B [n_id_B ] = child_node_B
@@ -202,10 +202,11 @@ def calc_heuristic(n1, n2):
202
202
d = w * math .hypot (n1 .x - n2 .x , n1 .y - n2 .y )
203
203
return d
204
204
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
209
210
210
211
def calc_grid_position (self , index , minp ):
211
212
"""
0 commit comments