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

radard: tie radard frequency to modelV2 vol. 2 (#29240)

Browse Source 
* radard: tie radard frequency to modelV2

Accumulate parsed messages until state is updated (toyota)

Same for honda

Rename rr to something more descriptive

* Change _update method name, since signature has changed

* Update ref commit

* Some renames

* Check for number of cans
old-commit-hash: 460f5c8e0b2fd2c420e211b09b70d1d79834a929
This commit is contained in:
Kacper Rączy
2023-08-11 18:07:06 -07:00
committed by GitHub
parent 20f6a25a1c
commit 4017b1707c
11 changed files with 148 additions and 107 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -46,7 +46,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

@@ -51,7 +51,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)

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

@@ -1,4 +1,6 @@
#!/usr/bin/env python3
from collections import defaultdict
from cereal import car
from opendbc.can.parser import CANParser
from selfdrive.car.interfaces import RadarInterfaceBase
@@ -28,50 +30,61 @@ class RadarInterface(RadarInterfaceBase):
else:
self.rcp = _create_nidec_can_parser(CP.carFingerprint)
self.trigger_msg = 0x445
self.updated_messages = set()
self.updated_values = defaultdict(lambda: defaultdict(list))
def update(self, can_strings):
# 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)
vls = self.rcp.update_strings(can_strings)
self.updated_messages.update(vls)
if self.trigger_msg not in self.updated_messages:
return None
rr = self._update(self.updated_messages)
self.updated_messages.clear()
return rr
addresses = self.rcp.update_strings(can_strings)
for addr in addresses:
vals_dict = self.rcp.vl_all[addr]
for sig_name, vals in vals_dict.items():
self.updated_values[addr][sig_name].extend(vals)
def _update(self, updated_messages):
if self.trigger_msg not in self.updated_values:
return None
radar_data = self._radar_msg_from_buffer(self.updated_values, self.rcp.can_valid)
self.updated_values.clear()
return radar_data
def _radar_msg_from_buffer(self, updated_values, can_valid):
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]
for ii in sorted(updated_values):
msgs = updated_values[ii]
n_vals_per_addr = len(list(msgs.values())[0])
cpts = [
{k: v[i] for k, v in msgs.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:
if not can_valid:
errors.append("canError")
if self.radar_fault:
errors.append("fault")

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

@@ -30,7 +30,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
@@ -312,14 +311,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

@@ -36,7 +36,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

@@ -1,4 +1,6 @@
#!/usr/bin/env python3
from collections import defaultdict
from opendbc.can.parser import CANParser
from cereal import car
from selfdrive.car.toyota.values import DBC, TSS2_CAR
@@ -36,34 +38,47 @@ class RadarInterface(RadarInterfaceBase):
self.rcp = None if CP.radarUnavailable else _create_radar_can_parser(CP.carFingerprint)
self.trigger_msg = self.RADAR_B_MSGS[-1]
self.updated_messages = set()
self.updated_values = defaultdict(lambda: defaultdict(list))
def update(self, can_strings):
if self.rcp is None:
return super().update(None)
vls = self.rcp.update_strings(can_strings)
self.updated_messages.update(vls)
if self.trigger_msg not in self.updated_messages:
return None
rr = self._update(self.updated_messages)
self.updated_messages.clear()
addresses = self.rcp.update_strings(can_strings)
for addr in addresses:
vals_dict = self.rcp.vl_all[addr]
for sig_name, vals in vals_dict.items():
self.updated_values[addr][sig_name].extend(vals)
return rr
if self.trigger_msg not in self.updated_values:
return None
def _update(self, updated_messages):
radar_data = self._radar_msg_from_buffer(self.updated_values, self.rcp.can_valid)
self.updated_values.clear()
return radar_data
def _radar_msg_from_buffer(self, updated_values, can_valid):
ret = car.RadarData.new_message()
errors = []
if not self.rcp.can_valid:
if not can_valid:
errors.append("canError")
ret.errors = errors
for ii in sorted(updated_messages):
if ii in self.RADAR_A_MSGS:
cpt = self.rcp.vl[ii]
for ii in sorted(updated_values):
if ii not in self.RADAR_A_MSGS:
continue
radar_a_msgs = updated_values[ii]
radar_b_msgs = updated_values[ii+16]
n_vals_per_addr = len(list(radar_a_msgs.values())[0])
cpts = [
{k: v[i] for k, v in radar_a_msgs.items()}
for i in range(n_vals_per_addr)
]
for index, cpt in enumerate(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:
@@ -71,11 +86,15 @@ class RadarInterface(RadarInterfaceBase):
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']
n_b_scores = len(radar_b_msgs['SCORE'])
if n_b_scores > 0:
score_index = min(index, n_b_scores - 1)
score = radar_b_msgs['SCORE'][score_index]
else:
score = None
# 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 cpt['VALID'] or (score and 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

99
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,15 +164,17 @@ 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
@@ -174,7 +182,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:
@@ -204,14 +212,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
@@ -219,26 +227,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
@@ -291,32 +305,33 @@ 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)
if len(can_strings) == 0:
radar_data = None
else:
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

@@ -465,8 +465,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
b3c089c376bb34666110bb6944e8f0501f37780b