From 9322f80ea92ae574c94a9d75ee794035e1735cf1 Mon Sep 17 00:00:00 2001 From: infiniteCable <75014343+infiniteCable@users.noreply.github.com> Date: Sat, 19 Apr 2025 21:26:36 +0200 Subject: [PATCH] Update radard.py rework --- selfdrive/controls/radard.py | 297 ++++++++++++----------------------- 1 file changed, 102 insertions(+), 195 deletions(-) diff --git a/selfdrive/controls/radard.py b/selfdrive/controls/radard.py index bee424405..61297e0e2 100755 --- a/selfdrive/controls/radard.py +++ b/selfdrive/controls/radard.py @@ -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()