Change radar-based FCW to model-based FCW (#22379)
* just use MPC for fcw checking * thats already bad * model FCW is always good * better fcw * should be good for now * comment * linting * cleaner * unusedpull/22380/head
HaraldSchafer
GitHub
parent
425020a849
commit
a8b4249ebc
|
|
@ -235,7 +235,6 @@ selfdrive/controls/lib/pid.py
|
|||
selfdrive/controls/lib/longitudinal_planner.py
|
||||
selfdrive/controls/lib/radar_helpers.py
|
||||
selfdrive/controls/lib/vehicle_model.py
|
||||
selfdrive/controls/lib/fcw.py
|
||||
|
||||
selfdrive/controls/lib/cluster/*
|
||||
|
||||
|
|
|
|||
|
|
@ -270,7 +270,9 @@ class Controls:
|
|||
self.events.add(EventName.deviceFalling)
|
||||
if log.PandaState.FaultType.relayMalfunction in self.sm['pandaState'].faults:
|
||||
self.events.add(EventName.relayMalfunction)
|
||||
if self.sm['longitudinalPlan'].fcw or (self.enabled and self.sm['modelV2'].meta.hardBrakePredicted):
|
||||
planner_fcw = self.sm['longitudinalPlan'].fcw and self.enabled
|
||||
model_fcw = self.sm['modelV2'].meta.hardBrakePredicted and not CS.brakePressed
|
||||
if planner_fcw or model_fcw:
|
||||
self.events.add(EventName.fcw)
|
||||
|
||||
if TICI:
|
||||
|
|
|
|||
|
|
@ -1,76 +0,0 @@
|
|||
import math
|
||||
from collections import defaultdict
|
||||
|
||||
from common.numpy_fast import interp
|
||||
|
||||
_FCW_A_ACT_V = [-3., -2.]
|
||||
_FCW_A_ACT_BP = [0., 30.]
|
||||
|
||||
|
||||
class FCWChecker():
|
||||
def __init__(self):
|
||||
self.reset_lead(0.0)
|
||||
self.common_counters = defaultdict(lambda: 0)
|
||||
|
||||
def reset_lead(self, cur_time):
|
||||
self.last_fcw_a = 0.0
|
||||
self.v_lead_max = 0.0
|
||||
self.lead_seen_t = cur_time
|
||||
self.last_fcw_time = 0.0
|
||||
self.last_min_a = 0.0
|
||||
|
||||
self.counters = defaultdict(lambda: 0)
|
||||
|
||||
@staticmethod
|
||||
def calc_ttc(v_ego, a_ego, x_lead, v_lead, a_lead):
|
||||
max_ttc = 5.0
|
||||
|
||||
v_rel = v_ego - v_lead
|
||||
a_rel = a_ego - a_lead
|
||||
|
||||
# assuming that closing gap ARel comes from lead vehicle decel,
|
||||
# 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 = 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 (math.sqrt(delta) + v_rel < 0.1):
|
||||
ttc = max_ttc
|
||||
else:
|
||||
ttc = min(2 * x_lead / (math.sqrt(delta) + v_rel), max_ttc)
|
||||
return ttc
|
||||
|
||||
def update(self, mpc_solution_a, cur_time, active, v_ego, a_ego, x_lead, v_lead, a_lead, y_lead, vlat_lead, fcw_lead, blinkers):
|
||||
|
||||
self.last_min_a = min(mpc_solution_a)
|
||||
self.v_lead_max = max(self.v_lead_max, v_lead)
|
||||
|
||||
self.common_counters['blinkers'] = self.common_counters['blinkers'] + 10.0 / (20 * 3.0) if not blinkers else 0
|
||||
self.common_counters['v_ego'] = self.common_counters['v_ego'] + 1 if v_ego > 5.0 else 0
|
||||
|
||||
if (fcw_lead > 0.99):
|
||||
ttc = self.calc_ttc(v_ego, a_ego, x_lead, v_lead, a_lead)
|
||||
self.counters['ttc'] = self.counters['ttc'] + 1 if ttc < 2.5 else 0
|
||||
self.counters['v_lead_max'] = self.counters['v_lead_max'] + 1 if self.v_lead_max > 2.5 else 0
|
||||
self.counters['v_ego_lead'] = self.counters['v_ego_lead'] + 1 if v_ego > v_lead else 0
|
||||
self.counters['lead_seen'] = self.counters['lead_seen'] + 0.33
|
||||
self.counters['y_lead'] = self.counters['y_lead'] + 1 if abs(y_lead) < 1.0 else 0
|
||||
self.counters['vlat_lead'] = self.counters['vlat_lead'] + 1 if abs(vlat_lead) < 0.4 else 0
|
||||
|
||||
a_thr = interp(v_lead, _FCW_A_ACT_BP, _FCW_A_ACT_V)
|
||||
a_delta = min(mpc_solution_a[:15]) - min(0.0, a_ego)
|
||||
|
||||
future_fcw_allowed = all(c >= 10 for c in self.counters.values())
|
||||
future_fcw_allowed = future_fcw_allowed and all(c >= 10 for c in self.common_counters.values())
|
||||
future_fcw = (self.last_min_a < -3.0 or a_delta < a_thr) and future_fcw_allowed
|
||||
|
||||
if future_fcw and (self.last_fcw_time + 5.0 < cur_time):
|
||||
self.last_fcw_time = cur_time
|
||||
self.last_fcw_a = self.last_min_a
|
||||
return True
|
||||
|
||||
return False
|
||||
|
|
@ -30,7 +30,7 @@ X_EGO_E2E_COST = 100.
|
|||
A_EGO_COST = .1
|
||||
J_EGO_COST = .2
|
||||
DANGER_ZONE_COST = 1e3
|
||||
CRASH_DISTANCE = 1.5
|
||||
CRASH_DISTANCE = .5
|
||||
LIMIT_COST = 1e6
|
||||
T_IDXS = np.array(T_IDXS_LST)
|
||||
|
||||
|
|
@ -190,7 +190,7 @@ class LongitudinalMpc():
|
|||
self.status = False
|
||||
self.new_lead = False
|
||||
self.prev_lead_status = False
|
||||
self.crashing = False
|
||||
self.crash_cnt = 0.0
|
||||
self.prev_lead_x = 10
|
||||
self.solution_status = 0
|
||||
self.x0 = np.zeros(X_DIM)
|
||||
|
|
@ -259,7 +259,6 @@ class LongitudinalMpc():
|
|||
min_x_lead = ((v_ego + v_lead)/2) * (v_ego - v_lead) / (-MIN_ACCEL * 2)
|
||||
if x_lead < min_x_lead:
|
||||
x_lead = min_x_lead
|
||||
self.crashing = True
|
||||
|
||||
if (v_lead < 0.1 or -a_lead / 2.0 > v_lead):
|
||||
v_lead = 0.0
|
||||
|
|
@ -289,7 +288,6 @@ class LongitudinalMpc():
|
|||
|
||||
def update(self, carstate, radarstate, v_cruise):
|
||||
v_ego = self.x0[1]
|
||||
self.crashing = False
|
||||
self.status = radarstate.leadOne.status or radarstate.leadTwo.status
|
||||
|
||||
lead_xv_0 = self.process_lead(radarstate.leadOne)
|
||||
|
|
@ -319,8 +317,11 @@ class LongitudinalMpc():
|
|||
self.params[:,2] = np.min(x_obstacles, axis=1)
|
||||
|
||||
self.run()
|
||||
self.crashing = self.crashing or np.sum(lead_xv_0[:,0] - self.x_sol[:,0] < CRASH_DISTANCE) > 0
|
||||
|
||||
if (np.any(lead_xv_0[:,0] - self.x_sol[:,0] < CRASH_DISTANCE) and
|
||||
radarstate.leadOne.modelProb > 0.9):
|
||||
self.crash_cnt += 1
|
||||
else:
|
||||
self.crash_cnt = 0
|
||||
|
||||
def update_with_xva(self, x, v, a):
|
||||
self.yref[:,1] = x
|
||||
|
|
|
|||
|
|
@ -6,10 +6,8 @@ from common.numpy_fast import interp
|
|||
import cereal.messaging as messaging
|
||||
from cereal import log
|
||||
from common.realtime import DT_MDL
|
||||
from common.realtime import sec_since_boot
|
||||
from selfdrive.modeld.constants import T_IDXS
|
||||
from selfdrive.config import Conversions as CV
|
||||
from selfdrive.controls.lib.fcw import FCWChecker
|
||||
from selfdrive.controls.lib.longcontrol import LongCtrlState
|
||||
from selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import LongitudinalMpc
|
||||
from selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX, CONTROL_N
|
||||
|
|
@ -49,7 +47,6 @@ class Planner():
|
|||
self.mpc = LongitudinalMpc()
|
||||
|
||||
self.fcw = False
|
||||
self.fcw_checker = FCWChecker()
|
||||
|
||||
self.v_desired = init_v
|
||||
self.a_desired = init_a
|
||||
|
|
@ -63,7 +60,6 @@ class Planner():
|
|||
|
||||
|
||||
def update(self, sm, CP):
|
||||
cur_time = sec_since_boot()
|
||||
v_ego = sm['carState'].vEgo
|
||||
a_ego = sm['carState'].aEgo
|
||||
|
||||
|
|
@ -102,18 +98,10 @@ class Planner():
|
|||
self.a_desired_trajectory = self.mpc.a_solution[:CONTROL_N]
|
||||
self.j_desired_trajectory = self.mpc.j_solution[:CONTROL_N]
|
||||
|
||||
# determine fcw
|
||||
if self.mpc.new_lead:
|
||||
self.fcw_checker.reset_lead(cur_time)
|
||||
blinkers = sm['carState'].leftBlinker or sm['carState'].rightBlinker
|
||||
self.fcw = self.fcw_checker.update(self.mpc.x_sol[:,2], cur_time,
|
||||
sm['controlsState'].active,
|
||||
v_ego, sm['carState'].aEgo,
|
||||
self.lead_1.dRel, self.lead_1.vLead, self.lead_1.aLeadK,
|
||||
self.lead_1.yRel, self.lead_1.vLat,
|
||||
self.lead_1.fcw, blinkers) and not sm['carState'].brakePressed
|
||||
#TODO counter is only needed because radar is glitchy, remove once radar is gone
|
||||
self.fcw = self.mpc.crash_cnt > 5
|
||||
if self.fcw:
|
||||
cloudlog.info("FCW triggered %s", self.fcw_checker.counters)
|
||||
cloudlog.info("FCW triggered")
|
||||
|
||||
# Interpolate 0.05 seconds and save as starting point for next iteration
|
||||
a_prev = self.a_desired
|
||||
|
|
|
|||
Loading…
Reference in New Issue