Set directional masks using the primitive cost

jlbas · Dec 6, 2022 · 6090176 · 6090176
1 parent a393713
commit 6090176
Showing 1 changed file with 2 additions and 5 deletions.
diff --git a/lpsnav/utils/helper.py b/lpsnav/utils/helper.py
@@ -79,11 +79,8 @@ def dynamic_prim_cost(pos, pt, pt_speed, pt_vel, pred_line, line_th, line_vel, l
     if np.any(~in_front(pred_line[0], line_th, pt)):
         dir_costs = directed_cost_to_line(pos, pt_vel, line, line_vel)
         dir_masks = directed_masks(pos, pt_vel, line, line_vel, dir_costs)
-        return np.where(
-            ~in_front(pred_line[0], line_th, pt),
-            np.where(dir_masks, 0, np.inf),
-            prim_costs,
-        )
+        masked = np.where(dir_masks, 0, prim_costs)
+        return np.where(~in_front(pred_line[0], line_th, pt), masked, prim_costs)
     return prim_costs