From 09aad992288a8971e0ad5be2933dda9d3f4edff5 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Thu, 8 Oct 2020 07:23:53 -0500 Subject: [PATCH] Use math instead of numpy for calculating TTC, on average 80% faster (#2293) * Use math instead of numpy, on average 80% faster * whoops old-commit-hash: 6133f603f046bb08c8e3493c3092c796156b8930 --- selfdrive/controls/lib/fcw.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/selfdrive/controls/lib/fcw.py b/selfdrive/controls/lib/fcw.py index b7e53b9ccf..e582f54f5f 100644 --- a/selfdrive/controls/lib/fcw.py +++ b/selfdrive/controls/lib/fcw.py @@ -1,4 +1,4 @@ -import numpy as np +import math from collections import defaultdict from common.numpy_fast import interp @@ -32,16 +32,16 @@ class FCWChecker(): # then limit ARel so that v_lead will get to zero in no sooner than t_decel. # This helps underweighting ARel when v_lead is close to zero. t_decel = 2. - a_rel = np.minimum(a_rel, v_lead / t_decel) + a_rel = min(a_rel, v_lead / t_decel) # delta of the quadratic equation to solve for ttc delta = v_rel**2 + 2 * x_lead * a_rel # assign an arbitrary high ttc value if there is no solution to ttc - if delta < 0.1 or (np.sqrt(delta) + v_rel < 0.1): + if delta < 0.1 or (math.sqrt(delta) + v_rel < 0.1): ttc = max_ttc else: - ttc = np.minimum(2 * x_lead / (np.sqrt(delta) + v_rel), max_ttc) + ttc = min(2 * x_lead / (math.sqrt(delta) + v_rel), max_ttc) return ttc def update(self, mpc_solution, cur_time, active, v_ego, a_ego, x_lead, v_lead, a_lead, y_lead, vlat_lead, fcw_lead, blinkers):