mawei/openpilot-evo
1
0
Fork 0
mirror of https://github.com/infiniteCable2/openpilot.git synced 2026-08-06 00:36:29 +08:00
Code Issues Packages Projects Releases Wiki Activity

Update radard.py rework

Browse Source
This commit is contained in:
infiniteCable
2025-04-19 21:26:36 +02:00
committed by GitHub
parent 681067d852
commit 9322f80ea9
1 changed files with 102 additions and 195 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/controls/radard.py
+102 -195
View File
@@ -1,5 +1,6 @@
#!/usr/bin/env python3
import math
import time
import numpy as np
from collections import deque
from typing import Any
@@ -16,30 +17,23 @@ from opendbc.car import structs
from opendbc.car.hyundai.values import HyundaiFlags
from opendbc.sunnypilot.car.hyundai.values import HyundaiFlagsSP
# Constants
_LEAD_ACCEL_TAU = 1.0
TAU_GROW = 1.05
TAU_SHRINK = 0.90
TAU_MIN = 0.4
BLEND_KF = 0.2
BLEND_VREL_DERIV = 0.3
V_EGO_STATIONARY = 4.0
RADAR_TO_CAMERA = 1.52
# Default lead acceleration decay set to 50% at 1s
_LEAD_ACCEL_TAU = 1.5
# radar tracks
SPEED, ACCEL = 0, 1 # Kalman filter states enum
# stationary qualification parameters
V_EGO_STATIONARY = 4. # no stationary object flag below this speed
RADAR_TO_CENTER = 2.7 # (deprecated) RADAR is ~ 2.7m ahead from center of car
RADAR_TO_CAMERA = 1.52 # RADAR is ~ 1.5m ahead from center of mesh frame
SPEED, ACCEL = 0, 1
class KalmanParams:
def __init__(self, dt: float):
# Lead Kalman Filter params, calculating K from A, C, Q, R requires the control library.
# hardcoding a lookup table to compute K for values of radar_ts between 0.01s and 0.2s
assert dt > .01 and dt < .2, "Radar time step must be between .01s and 0.2s"
assert 0.01 < dt < 0.2
self.A = [[1.0, dt], [0.0, 1.0]]
self.C = [1.0, 0.0]
#Q = np.matrix([[10., 0.0], [0.0, 100.]])
#R = 1e3
#K = np.matrix([[ 0.05705578], [ 0.03073241]])
dts = [i * 0.01 for i in range(1, 21)]
K0 = [0.12287673, 0.14556536, 0.16522756, 0.18281627, 0.1988689, 0.21372394,
0.22761098, 0.24069424, 0.253096, 0.26491023, 0.27621103, 0.28705801,
@@ -51,248 +45,161 @@ class KalmanParams:
0.26393339, 0.26278425]
self.K = [[np.interp(dt, dts, K0)], [np.interp(dt, dts, K1)]]
class Track:
def __init__(self, identifier: int, v_lead: float, kalman_params: KalmanParams):
self.identifier = identifier
self.cnt = 0
self.aLeadTau = FirstOrderFilter(_LEAD_ACCEL_TAU, 0.45, DT_MDL)
self.K_A = kalman_params.A
self.K_C = kalman_params.C
self.K_K = kalman_params.K
self.kf = KF1D([[v_lead], [0.0]], self.K_A, self.K_C, self.K_K)
self.aLeadTau = _LEAD_ACCEL_TAU
self.kf = KF1D([[v_lead], [0.0]], kalman_params.A, kalman_params.C, kalman_params.K)
def update(self, d_rel: float, y_rel: float, v_rel: float, v_lead: float, measured: float):
# relative values, copy
self.dRel = d_rel # LONG_DIST
self.yRel = y_rel # -LAT_DIST
self.vRel = v_rel # REL_SPEED
def update(self, d_rel, y_rel, v_rel, v_lead, measured):
self.dRel = d_rel
self.yRel = y_rel
self.vRel = v_rel
self.vLead = v_lead
self.measured = measured # measured or estimate
# computed velocity and accelerations
self.measured = measured
if self.cnt > 0:
self.kf.update(self.vLead)
self.vLeadK = float(self.kf.x[SPEED][0])
self.aLeadK = float(self.kf.x[ACCEL][0])
# Learn if constant acceleration
if abs(self.aLeadK) < 0.5:
self.aLeadTau.x = _LEAD_ACCEL_TAU
else:
self.aLeadTau.update(0.0)
self.aLeadTau = min(max(self.aLeadTau, 0.05) * TAU_GROW, _LEAD_ACCEL_TAU) if abs(self.aLeadK) < 0.5 else max(self.aLeadTau * TAU_SHRINK, TAU_MIN)
self.cnt += 1
def get_RadarState(self, model_prob: float = 0.0):
def get_RadarState(self, model_prob=0.0):
return {
"dRel": float(self.dRel),
"yRel": float(self.yRel),
"vRel": float(self.vRel),
"vLead": float(self.vLead),
"vLeadK": float(self.vLeadK),
"aLeadK": float(self.aLeadK),
"aLeadTau": float(self.aLeadTau.x),
"dRel": self.dRel,
"yRel": self.yRel,
"vRel": self.vRel,
"vLead": self.vLead,
"vLeadK": self.vLeadK,
"aLeadK": self.aLeadK,
"aLeadTau": self.aLeadTau,
"status": True,
"fcw": self.is_potential_fcw(model_prob),
"fcw": model_prob > .9,
"modelProb": model_prob,
"radar": True,
"radarTrackId": self.identifier,
}
def potential_low_speed_lead(self, v_ego: float):
# stop for stuff in front of you and low speed, even without model confirmation
# Radar points closer than 0.75, are almost always glitches on toyota radars
return abs(self.yRel) < 1.0 and (v_ego < V_EGO_STATIONARY) and (0.75 < self.dRel < 25)
def potential_low_speed_lead(self, v_ego):
return abs(self.yRel) < 1.0 and v_ego < V_EGO_STATIONARY and 0.75 < self.dRel < 25
def is_potential_fcw(self, model_prob: float):
return model_prob > .9
def __str__(self):
ret = f"x: {self.dRel:4.1f} y: {self.yRel:4.1f} v: {self.vRel:4.1f} a: {self.aLeadK:4.1f}"
return ret
def laplacian_pdf(x: float, mu: float, b: float):
def laplacian_pdf(x, mu, b):
b = max(b, 1e-4)
return math.exp(-abs(x-mu)/b)
return math.exp(-abs(x - mu) / b)
def match_vision_to_track(v_ego, lead, tracks):
offset_d = lead.x[0] - RADAR_TO_CAMERA
def prob(track):
return laplacian_pdf(track.dRel, offset_d, lead.xStd[0]) * \
laplacian_pdf(track.yRel, -lead.y[0], lead.yStd[0]) * \
laplacian_pdf(track.vRel + v_ego, lead.v[0], lead.vStd[0])
best_track = max(tracks.values(), key=prob)
if abs(best_track.dRel - offset_d) < max(offset_d * .25, 5.0) and \
(abs(best_track.vRel + v_ego - lead.v[0]) < 10 or v_ego + best_track.vRel > 3):
return best_track
return None
def match_vision_to_track(v_ego: float, lead: capnp._DynamicStructReader, tracks: dict[int, Track]):
offset_vision_dist = lead.x[0] - RADAR_TO_CAMERA
def prob(c):
prob_d = laplacian_pdf(c.dRel, offset_vision_dist, lead.xStd[0])
prob_y = laplacian_pdf(c.yRel, -lead.y[0], lead.yStd[0])
prob_v = laplacian_pdf(c.vRel + v_ego, lead.v[0], lead.vStd[0])
# This isn't exactly right, but it's a good heuristic
return prob_d * prob_y * prob_v
track = max(tracks.values(), key=prob)
# if no 'sane' match is found return -1
# stationary radar points can be false positives
dist_sane = abs(track.dRel - offset_vision_dist) < max([(offset_vision_dist)*.25, 5.0])
vel_sane = (abs(track.vRel + v_ego - lead.v[0]) < 10) or (v_ego + track.vRel > 3)
if dist_sane and vel_sane:
return track
else:
return None
def get_RadarState_from_vision(lead_msg: capnp._DynamicStructReader, v_ego: float, model_v_ego: float):
lead_v_rel_pred = lead_msg.v[0] - model_v_ego
def get_RadarState_from_vision(lead_msg, v_ego, model_v_ego):
now = time.monotonic()
dt = now - getattr(get_RadarState_from_vision, "prev_ts", now)
get_RadarState_from_vision.prev_ts = now
d_rel = lead_msg.x[0] - RADAR_TO_CAMERA
v_rel = lead_msg.v[0] - model_v_ego
v_rel_deriv = (d_rel - getattr(get_RadarState_from_vision, "last_d", d_rel)) / dt if dt > 1e-3 else None
get_RadarState_from_vision.last_d = d_rel
v_rel_pred = (1.0 - BLEND_VREL_DERIV) * v_rel + BLEND_VREL_DERIV * v_rel_deriv if v_rel_deriv else v_rel
prev_a = getattr(get_RadarState_from_vision, "prev_aLeadK", 0.0)
a_blend = (1.0 - BLEND_KF) * (lead_msg.a[0] if len(lead_msg.a) else 0.0) + BLEND_KF * prev_a
get_RadarState_from_vision.prev_aLeadK = a_blend
return {
"dRel": float(lead_msg.x[0] - RADAR_TO_CAMERA),
"yRel": float(-lead_msg.y[0]),
"vRel": float(lead_v_rel_pred),
"vLead": float(v_ego + lead_v_rel_pred),
"vLeadK": float(v_ego + lead_v_rel_pred),
"aLeadK": float(lead_msg.a[0]),
"dRel": d_rel,
"yRel": -lead_msg.y[0],
"vRel": v_rel_pred,
"vLead": v_ego + v_rel_pred,
"vLeadK": v_ego + v_rel_pred,
"aLeadK": a_blend,
"aLeadTau": 0.3,
"fcw": False,
"modelProb": float(lead_msg.prob),
"modelProb": lead_msg.prob,
"status": True,
"radar": False,
"radarTrackId": -1,
}
def get_lead(v_ego: float, ready: bool, tracks: dict[int, Track], lead_msg: capnp._DynamicStructReader,
model_v_ego: float, CP: structs.CarParams, CP_SP: structs.CarParamsSP, low_speed_override: bool = True) -> dict[str, Any]:
# Determine leads, this is where the essential logic happens
if len(tracks) > 0 and ready and lead_msg.prob > .5:
track = match_vision_to_track(v_ego, lead_msg, tracks)
else:
track = None
lead_dict = {'status': False}
if track is not None:
lead_dict = track.get_RadarState(lead_msg.prob)
lead_dict = get_custom_yrel(CP, CP_SP, lead_dict, lead_msg)
elif (track is None) and ready and (lead_msg.prob > .5):
lead_dict = get_RadarState_from_vision(lead_msg, v_ego, model_v_ego)
if low_speed_override:
low_speed_tracks = [c for c in tracks.values() if c.potential_low_speed_lead(v_ego)]
if len(low_speed_tracks) > 0:
closest_track = min(low_speed_tracks, key=lambda c: c.dRel)
# Only choose new track if it is actually closer than the previous one
if (not lead_dict['status']) or (closest_track.dRel < lead_dict['dRel']):
lead_dict = closest_track.get_RadarState()
return lead_dict
def get_custom_yrel(CP: structs.CarParams, CP_SP: structs.CarParamsSP, lead_dict: dict[str, Any],
lead_msg: capnp._DynamicStructReader) -> dict[str, Any]:
def get_custom_yrel(CP, CP_SP, lead_dict, lead_msg):
if CP.brand == "hyundai" and (CP_SP.flags & HyundaiFlagsSP.ENHANCED_SCC or
CP.flags & (HyundaiFlags.CANFD_CAMERA_SCC | HyundaiFlags.CAMERA_SCC)):
lead_dict['yRel'] = float(-lead_msg.y[0])
lead_dict["yRel"] = float(-lead_msg.y[0])
return lead_dict
def get_lead(v_ego, ready, tracks, lead_msg, model_v_ego, CP, CP_SP, low_speed_override=True):
track = match_vision_to_track(v_ego, lead_msg, tracks) if tracks and ready and lead_msg.prob > 0.5 else None
lead_dict = track.get_RadarState(lead_msg.prob) if track else (
get_RadarState_from_vision(lead_msg, v_ego, model_v_ego) if ready and lead_msg.prob > 0.5 else {"status": False})
if track and abs(track.dRel - (lead_msg.x[0] - RADAR_TO_CAMERA)) > 3.0:
lead_dict = get_custom_yrel(CP, CP_SP, track.get_RadarState(lead_msg.prob), lead_msg)
if low_speed_override:
low_speed_tracks = [t for t in tracks.values() if t.potential_low_speed_lead(v_ego)]
if low_speed_tracks:
closest = min(low_speed_tracks, key=lambda t: t.dRel)
if not lead_dict["status"] or closest.dRel < lead_dict["dRel"]:
lead_dict = closest.get_RadarState()
return lead_dict
class RadarD:
def __init__(self, CP: structs.CarParams, CP_SP: structs.CarParams, delay: float = 0.0):
self.CP = CP
self.CP_SP = CP_SP
self.current_time = 0.0
self.tracks: dict[int, Track] = {}
def __init__(self, CP, CP_SP, delay=0.0):
self.CP, self.CP_SP = CP, CP_SP
self.kalman_params = KalmanParams(DT_MDL)
self.v_ego = 0.0
self.v_ego_hist = deque([0.0], maxlen=int(round(delay / DT_MDL))+1)
self.tracks = {}
self.v_ego_hist = deque([0.0], maxlen=int(round(delay / DT_MDL)) + 1)
self.last_v_ego_frame = -1
self.radar_state: capnp._DynamicStructBuilder | None = None
self.radar_state_valid = False
self.ready = False
def update(self, sm: messaging.SubMaster, rr: car.RadarData):
def update(self, sm, rr):
self.ready = sm.seen['modelV2']
self.current_time = 1e-9*max(sm.logMonoTime.values())
if sm.recv_frame['carState'] != self.last_v_ego_frame:
self.v_ego = sm['carState'].vEgo
self.v_ego_hist.append(self.v_ego)
self.v_ego_hist.append(sm['carState'].vEgo)
self.last_v_ego_frame = sm.recv_frame['carState']
ar_pts = {pt.trackId: [pt.dRel, pt.yRel, pt.vRel, pt.measured] for pt in rr.points}
self.tracks = {i: t for i, t in self.tracks.items() if i in ar_pts}
# *** remove missing points from meta data ***
for ids in list(self.tracks.keys()):
if ids not in ar_pts:
self.tracks.pop(ids, None)
for i, (d, y, v, m) in ar_pts.items():
v_lead = v + self.v_ego_hist[0]
if i not in self.tracks:
self.tracks[i] = Track(i, v_lead, self.kalman_params)
self.tracks[i].update(d, y, v, v_lead, m)
# *** compute the tracks ***
for ids in ar_pts:
rpt = ar_pts[ids]
# align v_ego by a fixed time to align it with the radar measurement
v_lead = rpt[2] + self.v_ego_hist[0]
# create the track if it doesn't exist or it's a new track
if ids not in self.tracks:
self.tracks[ids] = Track(ids, v_lead, self.kalman_params)
self.tracks[ids].update(rpt[0], rpt[1], rpt[2], v_lead, rpt[3])
# *** publish radarState ***
self.radar_state_valid = sm.all_checks()
model_v_ego = sm['modelV2'].velocity.x[0] if len(sm['modelV2'].velocity.x) else sm['carState'].vEgo
leads = sm['modelV2'].leadsV3
self.radar_state = log.RadarState.new_message()
self.radar_state.mdMonoTime = sm.logMonoTime['modelV2']
self.radar_state.radarErrors = rr.errors
self.radar_state.carStateMonoTime = sm.logMonoTime['carState']
self.radar_state.radarErrors = rr.errors
self.radar_state.valid = sm.all_checks()
if len(leads) > 1:
self.radar_state.leadOne = get_lead(sm['carState'].vEgo, self.ready, self.tracks, leads[0], model_v_ego, self.CP, self.CP_SP)
self.radar_state.leadTwo = get_lead(sm['carState'].vEgo, self.ready, self.tracks, leads[1], model_v_ego, self.CP, self.CP_SP, low_speed_override=False)
if len(sm['modelV2'].velocity.x):
model_v_ego = sm['modelV2'].velocity.x[0]
else:
model_v_ego = self.v_ego
leads_v3 = sm['modelV2'].leadsV3
if len(leads_v3) > 1:
self.radar_state.leadOne = get_lead(self.v_ego, self.ready, self.tracks, leads_v3[0], model_v_ego, self.CP, self.CP_SP, low_speed_override=True)
self.radar_state.leadTwo = get_lead(self.v_ego, self.ready, self.tracks, leads_v3[1], model_v_ego, self.CP, self.CP_SP, low_speed_override=False)
def publish(self, pm):
pm.send("radarState", messaging.new_message("radarState", valid=True, radarState=self.radar_state))
def publish(self, pm: messaging.PubMaster):
assert self.radar_state is not None
radar_msg = messaging.new_message("radarState")
radar_msg.valid = self.radar_state_valid
radar_msg.radarState = self.radar_state
pm.send("radarState", radar_msg)
# fuses camera and radar data for best lead detection
def main() -> None:
def main():
config_realtime_process(5, Priority.CTRL_LOW)
# wait for stats about the car to come in from controls
cloudlog.info("radard is waiting for CarParams")
CP = messaging.log_from_bytes(Params().get("CarParams", block=True), car.CarParams)
cloudlog.info("radard got CarParams")
cloudlog.info("radard is waiting for CarParamsSP")
CP_SP = messaging.log_from_bytes(Params().get("CarParamsSP", block=True), custom.CarParamsSP)
cloudlog.info("radard got CarParamsSP")
cloudlog.info("radard got parameters")
# *** setup messaging
sm = messaging.SubMaster(['modelV2', 'carState', 'liveTracks'], poll='modelV2')
pm = messaging.PubMaster(['radarState'])
RD = RadarD(CP, CP_SP, CP.radarDelay)
while 1:
while True:
sm.update()
RD.update(sm, sm['liveTracks'])
RD.publish(pm)
if __name__ == "__main__":
main()