mawei/onepilot
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

radard: tie radard frequency to modelV2 (#29063)

Browse Source 
old-commit-hash: 255f5c13f2d8c6796fd960b248856f11a3c1b05a
This commit is contained in:
Kacper Rączy
2023-08-03 22:10:21 -07:00
committed by GitHub
parent faa9444a6a
commit 79fa322d4a
12 changed files with 121 additions and 103 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
cereal

Submodule cereal updated: 2689300b4a...774bf79f4d

2
selfdrive/car/chrysler/radar_interface.py
View File

@@ -52,7 +52,7 @@ class RadarInterface(RadarInterfaceBase):
def update(self, can_strings):
if self.rcp is None or self.CP.radarUnavailable:
return super().update(None)
return None
vls = self.rcp.update_strings(can_strings)
self.updated_messages.update(vls)

2
selfdrive/car/ford/radar_interface.py
View File

@@ -62,7 +62,7 @@ class RadarInterface(RadarInterfaceBase):
def update(self, can_strings):
if self.rcp is None:
return super().update(None)
return None
vls = self.rcp.update_strings(can_strings)
self.updated_messages.update(vls)

2
selfdrive/car/gm/radar_interface.py
View File

@@ -44,7 +44,7 @@ class RadarInterface(RadarInterfaceBase):
def update(self, can_strings):
if self.rcp is None:
return super().update(None)
return None
vls = self.rcp.update_strings(can_strings)
self.updated_messages.update(vls)

47
selfdrive/car/honda/radar_interface.py
View File

@@ -38,7 +38,7 @@ class RadarInterface(RadarInterfaceBase):
# in Bosch radar and we are only steering for now, so sleep 0.05s to keep
# radard at 20Hz and return no points
if self.radar_off_can:
return super().update(None)
return None
vls = self.rcp.update_strings(can_strings)
self.updated_messages.update(vls)
@@ -54,25 +54,32 @@ class RadarInterface(RadarInterfaceBase):
ret = car.RadarData.new_message()
for ii in sorted(updated_messages):
cpt = self.rcp.vl[ii]
if ii == 0x400:
# check for radar faults
self.radar_fault = cpt['RADAR_STATE'] != 0x79
self.radar_wrong_config = cpt['RADAR_STATE'] == 0x69
elif cpt['LONG_DIST'] < 255:
if ii not in self.pts or cpt['NEW_TRACK']:
self.pts[ii] = car.RadarData.RadarPoint.new_message()
self.pts[ii].trackId = self.track_id
self.track_id += 1
self.pts[ii].dRel = cpt['LONG_DIST'] # from front of car
self.pts[ii].yRel = -cpt['LAT_DIST'] # in car frame's y axis, left is positive
self.pts[ii].vRel = cpt['REL_SPEED']
self.pts[ii].aRel = float('nan')
self.pts[ii].yvRel = float('nan')
self.pts[ii].measured = True
else:
if ii in self.pts:
del self.pts[ii]
combined_vals = self.rcp.vl_all[ii]
n_vals_per_addr = len(list(combined_vals.values())[0])
cpts = [
{k: v[i] for k, v in combined_vals.items()}
for i in range(n_vals_per_addr)
]
for cpt in cpts:
if ii == 0x400:
# check for radar faults
self.radar_fault = cpt['RADAR_STATE'] != 0x79
self.radar_wrong_config = cpt['RADAR_STATE'] == 0x69
elif cpt['LONG_DIST'] < 255:
if ii not in self.pts or cpt['NEW_TRACK']:
self.pts[ii] = car.RadarData.RadarPoint.new_message()
self.pts[ii].trackId = self.track_id
self.track_id += 1
self.pts[ii].dRel = cpt['LONG_DIST'] # from front of car
self.pts[ii].yRel = -cpt['LAT_DIST'] # in car frame's y axis, left is positive
self.pts[ii].vRel = cpt['REL_SPEED']
self.pts[ii].aRel = float('nan')
self.pts[ii].yvRel = float('nan')
self.pts[ii].measured = True
else:
if ii in self.pts:
del self.pts[ii]
errors = []
if not self.rcp.can_valid:

2
selfdrive/car/hyundai/radar_interface.py
View File

@@ -42,7 +42,7 @@ class RadarInterface(RadarInterfaceBase):
def update(self, can_strings):
if self.radar_off_can or (self.rcp is None):
return super().update(None)
return None
vls = self.rcp.update_strings(can_strings)
self.updated_messages.update(vls)

8
selfdrive/car/interfaces.py
View File

@@ -1,6 +1,5 @@
import yaml
import os
import time
from abc import abstractmethod, ABC
from typing import Any, Dict, Optional, Tuple, List, Callable
@@ -311,14 +310,9 @@ class RadarInterfaceBase(ABC):
self.rcp = None
self.pts = {}
self.delay = 0
self.radar_ts = CP.radarTimeStep
self.no_radar_sleep = 'NO_RADAR_SLEEP' in os.environ
def update(self, can_strings):
ret = car.RadarData.new_message()
if not self.no_radar_sleep:
time.sleep(self.radar_ts) # radard runs on RI updates
return ret
pass
class CarStateBase(ABC):

2
selfdrive/car/tesla/radar_interface.py
View File

@@ -58,7 +58,7 @@ class RadarInterface(RadarInterfaceBase):
def update(self, can_strings):
if self.rcp is None:
return super().update(None)
return None
values = self.rcp.update_strings(can_strings)
self.updated_messages.update(values)

55
selfdrive/car/toyota/radar_interface.py
View File

@@ -45,7 +45,7 @@ class RadarInterface(RadarInterfaceBase):
def update(self, can_strings):
if self.rcp is None:
return super().update(None)
return None
vls = self.rcp.update_strings(can_strings)
self.updated_messages.update(vls)
@@ -67,33 +67,38 @@ class RadarInterface(RadarInterfaceBase):
for ii in sorted(updated_messages):
if ii in self.RADAR_A_MSGS:
cpt = self.rcp.vl[ii]
combined_vals = self.rcp.vl_all[ii]
n_vals_per_addr = len(list(combined_vals.values())[0])
cpts = [
{k: v[i] for k, v in combined_vals.items()}
for i in range(n_vals_per_addr)
]
if cpt['LONG_DIST'] >= 255 or cpt['NEW_TRACK']:
self.valid_cnt[ii] = 0 # reset counter
if cpt['VALID'] and cpt['LONG_DIST'] < 255:
self.valid_cnt[ii] += 1
else:
self.valid_cnt[ii] = max(self.valid_cnt[ii] - 1, 0)
for cpt in cpts:
if cpt['LONG_DIST'] >= 255 or cpt['NEW_TRACK']:
self.valid_cnt[ii] = 0 # reset counter
if cpt['VALID'] and cpt['LONG_DIST'] < 255:
self.valid_cnt[ii] += 1
else:
self.valid_cnt[ii] = max(self.valid_cnt[ii] - 1, 0)
score = self.rcp.vl[ii+16]['SCORE']
# print ii, self.valid_cnt[ii], score, cpt['VALID'], cpt['LONG_DIST'], cpt['LAT_DIST']
score = self.rcp.vl[ii+16]['SCORE']
# radar point only valid if it's a valid measurement and score is above 50
if cpt['VALID'] or (score > 50 and cpt['LONG_DIST'] < 255 and self.valid_cnt[ii] > 0):
if ii not in self.pts or cpt['NEW_TRACK']:
self.pts[ii] = car.RadarData.RadarPoint.new_message()
self.pts[ii].trackId = self.track_id
self.track_id += 1
self.pts[ii].dRel = cpt['LONG_DIST'] # from front of car
self.pts[ii].yRel = -cpt['LAT_DIST'] # in car frame's y axis, left is positive
self.pts[ii].vRel = cpt['REL_SPEED']
self.pts[ii].aRel = float('nan')
self.pts[ii].yvRel = float('nan')
self.pts[ii].measured = bool(cpt['VALID'])
else:
if ii in self.pts:
del self.pts[ii]
# radar point only valid if it's a valid measurement and score is above 50
if cpt['VALID'] or (score > 50 and cpt['LONG_DIST'] < 255 and self.valid_cnt[ii] > 0):
if ii not in self.pts or cpt['NEW_TRACK']:
self.pts[ii] = car.RadarData.RadarPoint.new_message()
self.pts[ii].trackId = self.track_id
self.track_id += 1
self.pts[ii].dRel = cpt['LONG_DIST'] # from front of car
self.pts[ii].yRel = -cpt['LAT_DIST'] # in car frame's y axis, left is positive
self.pts[ii].vRel = cpt['REL_SPEED']
self.pts[ii].aRel = float('nan')
self.pts[ii].yvRel = float('nan')
self.pts[ii].measured = bool(cpt['VALID'])
else:
if ii in self.pts:
del self.pts[ii]
ret.points = list(self.pts.values())
return ret

96
selfdrive/controls/radard.py
View File

@@ -8,7 +8,7 @@ import capnp
from cereal import messaging, log, car
from common.numpy_fast import interp
from common.params import Params
from common.realtime import Ratekeeper, Priority, config_realtime_process
from common.realtime import Ratekeeper, Priority, config_realtime_process, DT_MDL
from system.swaglog import cloudlog
from common.kalman.simple_kalman import KF1D
@@ -50,7 +50,8 @@ class KalmanParams:
class Track:
def __init__(self, v_lead: float, kalman_params: KalmanParams):
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
@@ -63,9 +64,13 @@ class Track:
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
self.update_vlead(v_lead)
def update_vlead(self, v_lead: float):
self.vLead = v_lead
# computed velocity and accelerations
if self.cnt > 0:
self.kf.update(self.vLead)
@@ -98,11 +103,12 @@ class Track:
"vLead": float(self.vLead),
"vLeadK": float(self.vLeadK),
"aLeadK": float(self.aLeadK),
"aLeadTau": float(self.aLeadTau),
"status": True,
"fcw": self.is_potential_fcw(model_prob),
"modelProb": model_prob,
"radar": True,
"aLeadTau": float(self.aLeadTau)
"radarTrackId": self.identifier,
}
def potential_low_speed_lead(self, v_ego: float):
@@ -158,14 +164,16 @@ def get_RadarState_from_vision(lead_msg: capnp._DynamicStructReader, v_ego: floa
"aLeadTau": 0.3,
"fcw": False,
"modelProb": float(lead_msg.prob),
"status": True,
"radar": False,
"status": True
"radarTrackId": -1,
}
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:
lead_msg_empty = any([len(c) == 0 for c in [lead_msg.x, lead_msg.y, lead_msg.v]])
if len(tracks) > 0 and ready and not lead_msg_empty and lead_msg.prob > .5:
track = match_vision_to_track(v_ego, lead_msg, tracks)
else:
track = None
@@ -173,7 +181,7 @@ def get_lead(v_ego: float, ready: bool, tracks: Dict[int, Track], lead_msg: capn
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):
elif track is None and ready and not lead_msg_empty and lead_msg.prob > .5:
lead_dict = get_RadarState_from_vision(lead_msg, v_ego, model_v_ego)
if low_speed_override:
@@ -203,14 +211,14 @@ class RadarD:
self.ready = False
def update(self, sm: messaging.SubMaster, rr: Optional[car.RadarData]):
def update(self, sm: messaging.SubMaster, radar_data: Optional[car.RadarData]):
self.current_time = 1e-9*max(sm.logMonoTime.values())
radar_points = []
radar_errors = []
if rr is not None:
radar_points = rr.points
radar_errors = rr.errors
if radar_data is not None:
radar_points = radar_data.points
radar_errors = radar_data.errors
if sm.updated['carState']:
self.v_ego = sm['carState'].vEgo
@@ -218,26 +226,32 @@ class RadarD:
if sm.updated['modelV2']:
self.ready = True
ar_pts = {}
for pt in radar_points:
ar_pts[pt.trackId] = [pt.dRel, pt.yRel, pt.vRel, pt.measured]
if radar_data is not None:
ar_pts = {}
for pt in radar_points:
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)
# *** 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]
# *** 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]
# 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(v_lead, self.kalman_params)
self.tracks[ids].update(rpt[0], rpt[1], rpt[2], v_lead, rpt[3])
# 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])
else:
# *** no radar points, keep existing tracks, update v_lead
for track in self.tracks.values():
v_lead = track.vRel + self.v_ego_hist[0]
track.update_vlead(v_lead)
# *** publish radarState ***
self.radar_state_valid = sm.all_checks() and len(radar_errors) == 0
@@ -290,32 +304,30 @@ def radard_thread(sm: Optional[messaging.SubMaster] = None, pm: Optional[messagi
cloudlog.info("radard is importing %s", CP.carName)
RadarInterface = importlib.import_module(f'selfdrive.car.{CP.carName}.radar_interface').RadarInterface
# *** setup messaging
# setup messaging
if can_sock is None:
can_sock = messaging.sub_sock('can')
if sm is None:
sm = messaging.SubMaster(['modelV2', 'carState'], ignore_avg_freq=['modelV2', 'carState']) # Can't check average frequency, since radar determines timing
sm = messaging.SubMaster(['modelV2', 'carState'], poll=["modelV2"])
if pm is None:
pm = messaging.PubMaster(['radarState', 'liveTracks'])
RI = RadarInterface(CP)
interface = RadarInterface(CP)
rk = Ratekeeper(1.0 / CP.radarTimeStep, print_delay_threshold=None)
RD = RadarD(CP.radarTimeStep, RI.delay)
rk = Ratekeeper(1 / DT_MDL, print_delay_threshold=None)
radar = RadarD(DT_MDL, interface.delay)
while 1:
can_strings = messaging.drain_sock_raw(can_sock, wait_for_one=True)
rr = RI.update(can_strings)
while True:
sm.update()
if rr is None:
continue
if sm.updated['modelV2']:
can_strings = messaging.drain_sock_raw(can_sock)
radar_data = interface.update(can_strings)
sm.update(0)
radar.update(sm, radar_data)
radar.publish(pm, -rk.remaining*1000.0)
RD.update(sm, rr)
RD.publish(pm, -rk.remaining*1000.0)
rk.monitor_time()
rk.monitor_time()
def main(sm: Optional[messaging.SubMaster] = None, pm: Optional[messaging.PubMaster] = None, can_sock: messaging.SubSocket = None):

4
selfdrive/test/process_replay/process_replay.py
View File

@@ -464,8 +464,8 @@ CONFIGS = [
subs=["radarState", "liveTracks"],
ignore=["logMonoTime", "valid", "radarState.cumLagMs"],
init_callback=get_car_params_callback,
should_recv_callback=MessageBasedRcvCallback("can"),
main_pub="can",
should_recv_callback=MessageBasedRcvCallback("modelV2"),
unlocked_pubs=["can"],
),
ProcessConfig(
proc_name="plannerd",

2
selfdrive/test/process_replay/ref_commit
View File

@@ -1 +1 @@
8fd42f02d1ecac695327bfc0afd4be1918ab680a
4e55314f3a2502dea71d84e23d4889724c7bd05e