sadmen
/
openpilot_comma
mirror of https://github.com/commaai/openpilot.git
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: 460f5c8e0b
beeps
Kacper Rączy 2 years ago committed by GitHub
parent 20f6a25a1c
commit 4017b1707c
11 changed files with 144 additions and 103 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 2
      selfdrive/car/chrysler/radar_interface.py
  2. 2
      selfdrive/car/ford/radar_interface.py
  3. 2
      selfdrive/car/gm/radar_interface.py
  4. 73
      selfdrive/car/honda/radar_interface.py
  5. 2
      selfdrive/car/hyundai/radar_interface.py
  6. 8
      selfdrive/car/interfaces.py
  7. 2
      selfdrive/car/tesla/radar_interface.py
  8. 51
      selfdrive/car/toyota/radar_interface.py
  9. 99
      selfdrive/controls/radard.py
  10. 4
      selfdrive/test/process_replay/process_replay.py
  11. 2
      selfdrive/test/process_replay/ref_commit

2
selfdrive/car/chrysler/radar_interface.py
Unescape 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
Unescape 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
Unescape 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)

73
selfdrive/car/honda/radar_interface.py
Unescape 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)
return None
vls = self.rcp.update_strings(can_strings)
self.updated_messages.update(vls)
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)
if self.trigger_msg not in self.updated_messages:
if self.trigger_msg not in self.updated_values:
return None
rr = self._update(self.updated_messages)
self.updated_messages.clear()
return rr
radar_data = self._radar_msg_from_buffer(self.updated_values, self.rcp.can_valid)
self.updated_values.clear()
return radar_data
def _update(self, updated_messages):
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
Unescape 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
Unescape 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
Unescape 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)

51
selfdrive/car/toyota/radar_interface.py
Unescape 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)
return None
vls = self.rcp.update_strings(can_strings)
self.updated_messages.update(vls)
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)
if self.trigger_msg not in self.updated_messages:
if self.trigger_msg not in self.updated_values:
return None
rr = self._update(self.updated_messages)
self.updated_messages.clear()
radar_data = self._radar_msg_from_buffer(self.updated_values, self.rcp.can_valid)
self.updated_values.clear()
return rr
return radar_data
def _update(self, updated_messages):
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
Unescape 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)
rk = Ratekeeper(1.0 / CP.radarTimeStep, print_delay_threshold=None)
RD = RadarD(CP.radarTimeStep, RI.delay)
interface = RadarInterface(CP)
while 1:
can_strings = messaging.drain_sock_raw(can_sock, wait_for_one=True)
rr = RI.update(can_strings)
rk = Ratekeeper(1 / DT_MDL, print_delay_threshold=None)
radar = RadarD(DT_MDL, interface.delay)
if rr is None:
continue
while True:
sm.update()
sm.update(0)
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)
RD.update(sm, rr)
RD.publish(pm, -rk.remaining*1000.0)
radar.update(sm, radar_data)
radar.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
Unescape 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
Unescape View File

@ -1 +1 @@
8fd42f02d1ecac695327bfc0afd4be1918ab680a
b3c089c376bb34666110bb6944e8f0501f37780b
Write Preview
Loading…
Cancel
Save