574 lines
21 KiB
Python
Executable File
574 lines
21 KiB
Python
Executable File
#!/usr/bin/env python3
|
|
import math
|
|
import numpy as np
|
|
from collections import deque
|
|
from typing import Any
|
|
|
|
import capnp
|
|
from cereal import messaging, log, car
|
|
from openpilot.common.params import Params
|
|
from openpilot.common.realtime import DT_MDL, Priority, config_realtime_process
|
|
from openpilot.common.swaglog import cloudlog
|
|
from openpilot.common.simple_kalman import KF1D
|
|
|
|
|
|
# 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
|
|
|
|
|
|
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"
|
|
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,
|
|
0.29750003, 0.30757767, 0.31732515, 0.32677158, 0.33594201, 0.34485814,
|
|
0.35353899, 0.36200124]
|
|
K1 = [0.29666309, 0.29330885, 0.29042818, 0.28787125, 0.28555364, 0.28342219,
|
|
0.28144091, 0.27958406, 0.27783249, 0.27617149, 0.27458948, 0.27307714,
|
|
0.27162685, 0.27023228, 0.26888809, 0.26758976, 0.26633338, 0.26511557,
|
|
0.26393339, 0.26278425]
|
|
self.K = [[np.interp(dt, dts, K0)], [np.interp(dt, dts, K1)]]
|
|
|
|
print("###KalmanParams.. : dt = ", dt)
|
|
|
|
|
|
class Track:
|
|
def __init__(self, identifier: int, v_lead: float, kalman_params: KalmanParams):
|
|
self.identifier = identifier
|
|
self.cnt = 0
|
|
self.aLeadTau = _LEAD_ACCEL_TAU
|
|
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.dRel = 0.0
|
|
self.vRel = 0.0
|
|
self.aLead = 0.0
|
|
self.vLead_last = v_lead
|
|
self.radar_reaction_factor = Params().get_float("RadarReactionFactor") * 0.01
|
|
|
|
def update(self, d_rel: float, y_rel: float, v_rel: float, v_lead: float, measured: float):
|
|
|
|
if abs(self.dRel - d_rel) > 3.0 or abs(self.vRel - v_rel) > 20.0 * DT_MDL:
|
|
self.cnt = 0
|
|
self.kf = KF1D([[v_lead], [0.0]], self.K_A, self.K_C, self.K_K)
|
|
self.aLead = 0.0
|
|
self.vLead_last = v_lead
|
|
|
|
# relative values, copy
|
|
self.dRel = d_rel # LONG_DIST
|
|
self.yRel = y_rel # -LAT_DIST
|
|
self.vRel = v_rel # REL_SPEED
|
|
self.vLead = v_lead
|
|
self.measured = measured # measured or estimate
|
|
|
|
# computed velocity and accelerations
|
|
if self.cnt > 0:
|
|
self.kf.update(self.vLead)
|
|
alpha = 0.15
|
|
#dv = 0.0 if abs(self.vLead) < 0.5 else self.vLead - self.vLead_last
|
|
dv = self.vLead - self.vLead_last
|
|
self.aLead = self.aLead * (1 - alpha) + dv / DT_MDL * alpha
|
|
|
|
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:
|
|
if abs(self.aLead) < 0.5 * self.radar_reaction_factor:
|
|
self.aLeadTau = _LEAD_ACCEL_TAU * self.radar_reaction_factor
|
|
else:
|
|
self.aLeadTau *= 0.9
|
|
|
|
self.cnt += 1
|
|
self.vLead_last = self.vLead
|
|
|
|
def reset_a_lead(self, aLeadK: float, aLeadTau: float):
|
|
self.kf = KF1D([[self.vLead], [aLeadK]], self.K_A, self.K_C, self.K_K)
|
|
self.aLeadK = aLeadK
|
|
self.aLeadTau = aLeadTau
|
|
|
|
def get_RadarState(self, md, model_prob: float = 0.0, vision_y_rel = 0.0):
|
|
yRel = vision_y_rel if vision_y_rel != 0.0 else float(self.yRel)
|
|
dPath = yRel + np.interp(self.dRel, md.position.x, md.position.y)
|
|
return {
|
|
"dRel": float(self.dRel),
|
|
"yRel": float(self.yRel) if vision_y_rel == 0.0 else vision_y_rel,
|
|
"dPath" : float(dPath),
|
|
"vRel": float(self.vRel),
|
|
"vLead": float(self.vLead),
|
|
"vLeadK": float(self.vLeadK),
|
|
"aLead": float(self.aLead),
|
|
"aLeadK": float(self.aLeadK),
|
|
"aLeadTau": float(self.aLeadTau),
|
|
"status": True,
|
|
"fcw": self.is_potential_fcw(model_prob),
|
|
"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 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):
|
|
b = max(b, 1e-4)
|
|
return math.exp(-abs(x-mu)/b)
|
|
|
|
|
|
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])
|
|
|
|
weight_v = np.interp(c.vRel + v_ego, [0, 10], [0.3, 1])
|
|
# This isn't exactly right, but it's a good heuristic
|
|
return prob_d * prob_y * prob_v * weight_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)
|
|
vel_tolerance = 25.0 if lead.prob > 0.99 else 10.0
|
|
dist_sane = abs(track.dRel - offset_vision_dist) < max([(offset_vision_dist)*.35, 5.0])
|
|
vel_sane = (abs(track.vRel + v_ego - lead.v[0]) < vel_tolerance) or (v_ego + track.vRel > 3)
|
|
if dist_sane and vel_sane:
|
|
return track
|
|
else:
|
|
return None
|
|
|
|
|
|
def get_RadarState_from_vision(md, lead_msg: capnp._DynamicStructReader, v_ego: float, model_v_ego: float):
|
|
lead_v_rel_pred = lead_msg.v[0] - model_v_ego
|
|
dRel = float(lead_msg.x[0] - RADAR_TO_CAMERA)
|
|
yRel = float(-lead_msg.y[0])
|
|
dPath = yRel + np.interp(dRel, md.position.x, md.position.y)
|
|
return {
|
|
"dRel": float(dRel),
|
|
"yRel": yRel,
|
|
"dPath" : float(dPath),
|
|
"vRel": float(lead_v_rel_pred),
|
|
"vLead": float(v_ego + lead_v_rel_pred),
|
|
"vLeadK": float(v_ego + lead_v_rel_pred),
|
|
"aLead": float(lead_msg.a[0]),
|
|
"aLeadK": float(lead_msg.a[0]),
|
|
"aLeadTau": 0.3,
|
|
"fcw": False,
|
|
"modelProb": float(lead_msg.prob),
|
|
"status": True,
|
|
"radar": False,
|
|
"radarTrackId": -1,
|
|
}
|
|
|
|
def get_lead_side(v_ego, tracks, md, lane_width, model_v_ego):
|
|
lead_msg = md.leadsV3[0]
|
|
leadCenter = {'status': False}
|
|
leadLeft = {'status': False}
|
|
leadRight = {'status': False}
|
|
|
|
## SCC레이더는 일단 보관하고 리스트에서 삭제...
|
|
track_scc = tracks.get(0)
|
|
#if track_scc is not None:
|
|
# del tracks[0]
|
|
|
|
#if len(tracks) == 0:
|
|
# return [[],[],[],leadLeft,leadRight]
|
|
if md is not None and len(md.position.x) == 33: #ModelConstants.IDX_N:
|
|
md_y = md.position.y
|
|
md_x = md.position.x
|
|
else:
|
|
return [[],[],[],leadCenter,leadLeft,leadRight]
|
|
|
|
leads_center = {}
|
|
leads_left = {}
|
|
leads_right = {}
|
|
next_lane_y = lane_width / 2 + lane_width * 0.8
|
|
for c in tracks.values():
|
|
# d_y : path_y - traks_y 의 diff값
|
|
# yRel값은 왼쪽이 +값, lead.y[0]값은 왼쪽이 -값
|
|
d_y = c.yRel + np.interp(c.dRel, md_x, md_y)
|
|
if abs(d_y) < lane_width/2:
|
|
ld = c.get_RadarState(md, lead_msg.prob, float(-lead_msg.y[0]))
|
|
leads_center[c.dRel] = ld
|
|
elif -next_lane_y < d_y < 0:
|
|
ld = c.get_RadarState(md, 0, 0)
|
|
leads_right[c.dRel] = ld
|
|
elif 0 < d_y < next_lane_y:
|
|
ld = c.get_RadarState(md, 0, 0)
|
|
leads_left[c.dRel] = ld
|
|
|
|
if lead_msg.prob > 0.5:
|
|
ld = get_RadarState_from_vision(md, lead_msg, v_ego, model_v_ego)
|
|
leads_center[ld['dRel']] = ld
|
|
#ll,lr = [[l[k] for k in sorted(list(l.keys()))] for l in [leads_left,leads_right]]
|
|
#lc = sorted(leads_center.values(), key=lambda c:c["dRel"])
|
|
ll = list(leads_left.values())
|
|
lr = list(leads_right.values())
|
|
|
|
if leads_center:
|
|
dRel_min = min(leads_center.keys())
|
|
lc = [leads_center[dRel_min]]
|
|
else:
|
|
lc = {}
|
|
|
|
leadLeft = min((lead for dRel, lead in leads_left.items() if lead['dRel'] > 5.0), key=lambda x: x['dRel'], default=leadLeft)
|
|
leadRight = min((lead for dRel, lead in leads_right.items() if lead['dRel'] > 5.0), key=lambda x: x['dRel'], default=leadRight)
|
|
leadCenter = min((lead for dRel, lead in leads_center.items() if lead['vLead'] > 10 / 3.6 and lead['radar']), key=lambda x: x['dRel'], default=leadCenter)
|
|
|
|
#filtered_leads_left = {dRel: lead for dRel, lead in leads_left.items() if lead['dRel'] > 5.0}
|
|
#if filtered_leads_left:
|
|
# dRel_min = min(filtered_leads_left.keys())
|
|
# leadLeft = filtered_leads_left[dRel_min]
|
|
|
|
#filtered_leads_right = {dRel: lead for dRel, lead in leads_right.items() if lead['dRel'] > 5.0}
|
|
#if filtered_leads_right:
|
|
# dRel_min = min(filtered_leads_right.keys())
|
|
# leadRight = filtered_leads_right[dRel_min]
|
|
|
|
return [ll, lc, lr, leadCenter, leadLeft, leadRight]
|
|
|
|
def get_lead(v_ego: float, ready: bool, tracks: dict[int, Track], lead_msg: capnp._DynamicStructReader,
|
|
model_v_ego: float, 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)
|
|
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
|
|
|
|
|
|
class VisionTrack:
|
|
def __init__(self, radar_ts):
|
|
self.radar_ts = radar_ts
|
|
self.dRel = 0.0
|
|
self.vRel = 0.0
|
|
self.yRel = 0.0
|
|
self.vLead = 0.0
|
|
self.aLead = 0.0
|
|
self.vLeadK = 0.0
|
|
self.aLeadK = 0.0
|
|
self.aLeadTau = _LEAD_ACCEL_TAU
|
|
self.prob = 0.0
|
|
self.status = False
|
|
|
|
self.dRel_last = 0.0
|
|
self.vLead_last = 0.0
|
|
self.alpha = 0.02
|
|
self.alpha_a = 0.02
|
|
|
|
self.vLat = 0.0
|
|
|
|
self.v_ego = 0.0
|
|
self.cnt = 0
|
|
|
|
self.dPath = 0.0
|
|
|
|
def get_lead(self, md):
|
|
#aLeadK = 0.0 if self.mixRadarInfo in [3] else clip(self.aLeadK, self.aLead - 1.0, self.aLead + 1.0)
|
|
return {
|
|
"dRel": self.dRel,
|
|
"yRel": self.yRel,
|
|
#"dPath": self.dPath,
|
|
"vRel": self.vRel,
|
|
"vLead": self.vLead,
|
|
"vLeadK": self.vLeadK, ## TODO: 아직 vLeadK는 엉망인듯...
|
|
"aLead": self.aLead,
|
|
"aLeadK": self.aLeadK,
|
|
"aLeadTau": self.aLeadTau,
|
|
"fcw": False,
|
|
"modelProb": self.prob,
|
|
"status": self.status,
|
|
"radar": False,
|
|
"radarTrackId": -1,
|
|
#"aLead": self.aLead,
|
|
#"vLat": self.vLat,
|
|
}
|
|
|
|
def reset(self):
|
|
self.status = False
|
|
self.aLeadTau = _LEAD_ACCEL_TAU
|
|
|
|
self.vRel = 0.0
|
|
self.vLead = self.vLeadK = self.v_ego
|
|
self.aLead = self.aLeadK = 0.0
|
|
self.vLat = 0.0
|
|
|
|
def update(self, lead_msg, model_v_ego, v_ego, md):
|
|
|
|
lead_v_rel_pred = lead_msg.v[0] - model_v_ego
|
|
self.prob = lead_msg.prob
|
|
self.v_ego = v_ego
|
|
if self.prob > .5:
|
|
dRel = float(lead_msg.x[0]) - RADAR_TO_CAMERA
|
|
if abs(self.dRel - dRel) > 5.0:
|
|
self.cnt = 0
|
|
self.dRel = dRel
|
|
|
|
self.yRel = float(-lead_msg.y[0])
|
|
dPath = self.yRel + np.interp(self.dRel, md.position.x, md.position.y)
|
|
a_lead_vision = lead_msg.a[0]
|
|
if self.cnt < 1 or self.prob < 0.99:
|
|
self.vRel = lead_v_rel_pred
|
|
self.vLead = float(v_ego + lead_v_rel_pred)
|
|
self.aLead = a_lead_vision
|
|
self.vLat = 0.0
|
|
else:
|
|
v_rel = (self.dRel - self.dRel_last) / self.radar_ts
|
|
v_rel = self.vRel * (1. - self.alpha) + v_rel * self.alpha
|
|
|
|
#self.vRel = lead_v_rel_pred if self.mixRadarInfo == 3 else (lead_v_rel_pred + self.vRel) / 2
|
|
self.vRel = (lead_v_rel_pred + v_rel) / 2
|
|
self.vLead = float(v_ego + self.vRel)
|
|
|
|
a_lead = (self.vLead - self.vLead_last) / self.radar_ts * 0.2 #0.5 -> 0.2 vel 미분적용을 줄임.
|
|
self.aLead = self.aLead * (1. - self.alpha_a) + a_lead * self.alpha_a
|
|
if abs(a_lead_vision) > abs(self.aLead): # or self.mixRadarInfo == 3:
|
|
self.aLead = a_lead_vision
|
|
|
|
vLat_alpha = 0.002
|
|
self.vLat = self.vLat * (1. - vLat_alpha) + (dPath - self.dPath) / self.radar_ts * vLat_alpha
|
|
|
|
self.dPath = dPath
|
|
|
|
self.vLeadK= self.vLead
|
|
self.aLeadK = self.aLead
|
|
|
|
self.status = True
|
|
self.cnt += 1
|
|
else:
|
|
self.reset()
|
|
self.cnt = 0
|
|
self.dPath = self.yRel + np.interp(v_ego ** 2 / (2 * 2.5), md.position.x, md.position.y)
|
|
|
|
self.dRel_last = self.dRel
|
|
self.vLead_last = self.vLead
|
|
|
|
# Learn if constant acceleration
|
|
#aLeadTauValue = self.aLeadTauPos if self.aLead > self.aLeadTauThreshold else self.aLeadTauNeg
|
|
if abs(self.aLead) < 0.3: #self.aLeadTauThreshold:
|
|
self.aLeadTau = 0.2 #aLeadTauValue
|
|
else:
|
|
#self.aLeadTau = min(self.aLeadTau * 0.9, aLeadTauValue)
|
|
self.aLeadTau *= 0.9
|
|
|
|
class RadarD:
|
|
def __init__(self, delay: float = 0.0):
|
|
self.current_time = 0.0
|
|
|
|
self.tracks: dict[int, Track] = {}
|
|
self.kalman_params = KalmanParams(DT_MDL)
|
|
|
|
self.v_ego = 0.0
|
|
print("###RadarD.. : delay = ", delay, int(round(delay / DT_MDL))+1)
|
|
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
|
|
|
|
self.vision_tracks = [VisionTrack(DT_MDL), VisionTrack(DT_MDL)]
|
|
|
|
|
|
def update(self, sm: messaging.SubMaster, rr: car.RadarData):
|
|
self.ready = sm.seen['modelV2']
|
|
self.current_time = 1e-9*max(sm.logMonoTime.values())
|
|
|
|
|
|
leads_v3 = sm['modelV2'].leadsV3
|
|
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.last_v_ego_frame = sm.recv_frame['carState']
|
|
|
|
ar_pts = {}
|
|
for pt in rr.points:
|
|
pt_yRel = pt.yRel
|
|
if pt.trackId == 0 and pt.yRel == 0: # scc radar
|
|
if self.ready and leads_v3[0].prob > 0.5:
|
|
pt_yRel = -leads_v3[0].y[0]
|
|
ar_pts[pt.trackId] = [pt.dRel, pt_yRel, pt.vRel, pt.measured]
|
|
|
|
# *** remove missing points from meta data ***
|
|
for ids in list(self.tracks.keys()):
|
|
if ids not in ar_pts:
|
|
self.tracks.pop(ids, None)
|
|
|
|
# *** 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() and len(rr.errors) == 0
|
|
self.radar_state = log.RadarState.new_message()
|
|
|
|
model_updated = False if self.radar_state.mdMonoTime == sm.logMonoTime['modelV2'] else True
|
|
|
|
self.radar_state.mdMonoTime = sm.logMonoTime['modelV2']
|
|
self.radar_state.radarErrors = list(rr.errors)
|
|
self.radar_state.carStateMonoTime = sm.logMonoTime['carState']
|
|
|
|
if len(sm['modelV2'].temporalPose.trans):
|
|
model_v_ego = sm['modelV2'].temporalPose.trans[0]
|
|
else:
|
|
model_v_ego = self.v_ego
|
|
#leads_v3 = sm['modelV2'].leadsV3
|
|
if len(leads_v3) > 1:
|
|
|
|
if model_updated:
|
|
self.vision_tracks[0].update(leads_v3[0], model_v_ego, self.v_ego, sm['modelV2'])
|
|
self.vision_tracks[1].update(leads_v3[1], model_v_ego, self.v_ego, sm['modelV2'])
|
|
|
|
#self.radar_state.leadOne = get_lead(self.v_ego, self.ready, self.tracks, leads_v3[0], model_v_ego, low_speed_override=False)
|
|
#self.radar_state.leadTwo = get_lead(self.v_ego, self.ready, self.tracks, leads_v3[1], model_v_ego, low_speed_override=False)
|
|
self.radar_state.leadOne = self.get_lead(sm['modelV2'], self.tracks, 0, leads_v3[0], model_v_ego, low_speed_override=False)
|
|
self.radar_state.leadTwo = self.get_lead(sm['modelV2'], self.tracks, 1, leads_v3[1], model_v_ego, low_speed_override=False)
|
|
|
|
# ll, lc, lr, leadCenter, self.radar_state.leadLeft, self.radar_state.leadRight = get_lead_side(self.v_ego, self.tracks, sm['modelV2'],
|
|
# sm['lateralPlan'].laneWidth, model_v_ego)
|
|
ll, lc, lr, leadCenter, self.radar_state.leadLeft, self.radar_state.leadRight = get_lead_side(self.v_ego, self.tracks, sm['modelV2'], 3.2, model_v_ego)
|
|
self.radar_state.leadsLeft = list(ll)
|
|
self.radar_state.leadsCenter = list(lc)
|
|
self.radar_state.leadsRight = list(lr)
|
|
|
|
|
|
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)
|
|
|
|
def get_lead(self, md, tracks: dict[int, Track], index: int, lead_msg: capnp._DynamicStructReader,
|
|
model_v_ego: float, low_speed_override: bool = True) -> dict[str, Any]:
|
|
|
|
v_ego = self.v_ego
|
|
ready = self.ready
|
|
## SCC레이더는 일단 보관하고 리스트에서 삭제...
|
|
track_scc = tracks.get(0)
|
|
#if track_scc is not None:
|
|
# del tracks[0] ## tracks에서 삭제하면안됨... ㅠㅠ
|
|
|
|
# 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
|
|
|
|
# vision match후 발견된 track이 없으면
|
|
# track_scc 가 있는 지 확인하고
|
|
# 비전과의 차이가 35%(5M)이상 차이나면 scc가 발견못한것이기 때문에 비전것으로 처리함.
|
|
|
|
### 240807, SCC레이더가 옆차선의것을 많이 가져옴... 사용하지 말아야겠다...
|
|
#if track_scc is not None and track is None:
|
|
# track = track_scc
|
|
# if self.vision_tracks[index].prob > .5:
|
|
# if self.vision_tracks[index].dRel < track.dRel - 10.0: #끼어드는 차량이 있는 경우 처리.. 5-> 10M바꿔보자... 240427
|
|
# track = None
|
|
|
|
lead_dict = {'status': False}
|
|
if track is not None:
|
|
#lead_dict = track.get_RadarState(md, lead_msg.prob, self.vision_tracks[0].yRel, self.vision_tracks[0].vLat)
|
|
lead_dict = track.get_RadarState(md, lead_msg.prob, self.vision_tracks[0].yRel)
|
|
elif (track is None) and ready and (lead_msg.prob > .5):
|
|
#if self.mixRadarInfo == 4 and v_ego * 3.6 > 30 and lead_msg.prob < 0.99: ##
|
|
# pass
|
|
#else:
|
|
lead_dict = self.vision_tracks[index].get_lead(md)
|
|
|
|
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(md, lead_msg.prob, self.vision_tracks[0].yRel, self.vision_tracks[0].vLat)
|
|
lead_dict = closest_track.get_RadarState(md, lead_msg.prob, self.vision_tracks[0].yRel)
|
|
|
|
return lead_dict
|
|
|
|
# fuses camera and radar data for best lead detection
|
|
def main() -> None:
|
|
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")
|
|
|
|
# *** setup messaging
|
|
sm = messaging.SubMaster(['modelV2', 'carState', 'liveTracks'], poll='modelV2')
|
|
pm = messaging.PubMaster(['radarState'])
|
|
|
|
RD = RadarD(CP.radarDelay)
|
|
|
|
while 1:
|
|
sm.update()
|
|
|
|
RD.update(sm, sm['liveTracks'])
|
|
RD.publish(pm)
|
|
|
|
|
|
if __name__ == "__main__":
|
|
main()
|