sadmen
/
openpilot_comma
mirror of https://github.com/commaai/openpilot.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

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

Browse Source 
* Revert "static analysis fixes"

This reverts commit 2271d0adf9a02aee1359717c47cb6280893a6b9b.

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

This reverts commit 6757e90415766e84ad1b6c921a2edb29ceb3bbbe.

* update refs
old-commit-hash: 1a08c2986e
beeps
Shane Smiskol 2 years ago committed by GitHub
parent 559c626fd1
commit 1a2818d775
11 changed files with 103 additions and 144 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified 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): def update(self, can_strings):
if self.rcp is None or self.CP.radarUnavailable: if self.rcp is None or self.CP.radarUnavailable:
return None return super().update(None)
vls = self.rcp.update_strings(can_strings) vls = self.rcp.update_strings(can_strings)
self.updated_messages.update(vls) 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): def update(self, can_strings):
if self.rcp is None: if self.rcp is None:
return None return super().update(None)
vls = self.rcp.update_strings(can_strings) vls = self.rcp.update_strings(can_strings)
self.updated_messages.update(vls) 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): def update(self, can_strings):
if self.rcp is None: if self.rcp is None:
return None return super().update(None)
vls = self.rcp.update_strings(can_strings) vls = self.rcp.update_strings(can_strings)
self.updated_messages.update(vls) self.updated_messages.update(vls)

73
selfdrive/car/honda/radar_interface.py
Unescape View File

@ -1,6 +1,4 @@
#!/usr/bin/env python3 #!/usr/bin/env python3
from collections import defaultdict
from cereal import car from cereal import car
from opendbc.can.parser import CANParser from opendbc.can.parser import CANParser
from selfdrive.car.interfaces import RadarInterfaceBase from selfdrive.car.interfaces import RadarInterfaceBase
@ -30,61 +28,50 @@ class RadarInterface(RadarInterfaceBase):
else: else:
self.rcp = _create_nidec_can_parser(CP.carFingerprint) self.rcp = _create_nidec_can_parser(CP.carFingerprint)
self.trigger_msg = 0x445 self.trigger_msg = 0x445
self.updated_values = defaultdict(lambda: defaultdict(list)) self.updated_messages = set()
def update(self, can_strings): def update(self, can_strings):
# in Bosch radar and we are only steering for now, so sleep 0.05s to keep # in Bosch radar and we are only steering for now, so sleep 0.05s to keep
# radard at 20Hz and return no points # radard at 20Hz and return no points
if self.radar_off_can: if self.radar_off_can:
return None return super().update(None)
addresses = self.rcp.update_strings(can_strings) vls = self.rcp.update_strings(can_strings)
for addr in addresses: self.updated_messages.update(vls)
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_values: if self.trigger_msg not in self.updated_messages:
return None return None
radar_data = self._radar_msg_from_buffer(self.updated_values, self.rcp.can_valid) rr = self._update(self.updated_messages)
self.updated_values.clear() self.updated_messages.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() ret = car.RadarData.new_message()
for ii in sorted(updated_values): for ii in sorted(updated_messages):
msgs = updated_values[ii] cpt = self.rcp.vl[ii]
n_vals_per_addr = len(list(msgs.values())[0]) if ii == 0x400:
cpts = [ # check for radar faults
{k: v[i] for k, v in msgs.items()} self.radar_fault = cpt['RADAR_STATE'] != 0x79
for i in range(n_vals_per_addr) self.radar_wrong_config = cpt['RADAR_STATE'] == 0x69
] elif cpt['LONG_DIST'] < 255:
if ii not in self.pts or cpt['NEW_TRACK']:
for cpt in cpts: self.pts[ii] = car.RadarData.RadarPoint.new_message()
if ii == 0x400: self.pts[ii].trackId = self.track_id
# check for radar faults self.track_id += 1
self.radar_fault = cpt['RADAR_STATE'] != 0x79 self.pts[ii].dRel = cpt['LONG_DIST'] # from front of car
self.radar_wrong_config = cpt['RADAR_STATE'] == 0x69 self.pts[ii].yRel = -cpt['LAT_DIST'] # in car frame's y axis, left is positive
elif cpt['LONG_DIST'] < 255: self.pts[ii].vRel = cpt['REL_SPEED']
if ii not in self.pts or cpt['NEW_TRACK']: self.pts[ii].aRel = float('nan')
self.pts[ii] = car.RadarData.RadarPoint.new_message() self.pts[ii].yvRel = float('nan')
self.pts[ii].trackId = self.track_id self.pts[ii].measured = True
self.track_id += 1 else:
self.pts[ii].dRel = cpt['LONG_DIST'] # from front of car if ii in self.pts:
self.pts[ii].yRel = -cpt['LAT_DIST'] # in car frame's y axis, left is positive del self.pts[ii]
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 = [] errors = []
if not can_valid: if not self.rcp.can_valid:
errors.append("canError") errors.append("canError")
if self.radar_fault: if self.radar_fault:
errors.append("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): def update(self, can_strings):
if self.radar_off_can or (self.rcp is None): if self.radar_off_can or (self.rcp is None):
return None return super().update(None)
vls = self.rcp.update_strings(can_strings) vls = self.rcp.update_strings(can_strings)
self.updated_messages.update(vls) self.updated_messages.update(vls)

8
selfdrive/car/interfaces.py
Unescape View File

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

51
selfdrive/car/toyota/radar_interface.py
Unescape View File

@ -1,6 +1,4 @@
#!/usr/bin/env python3 #!/usr/bin/env python3
from collections import defaultdict
from opendbc.can.parser import CANParser from opendbc.can.parser import CANParser
from cereal import car from cereal import car
from selfdrive.car.toyota.values import DBC, TSS2_CAR from selfdrive.car.toyota.values import DBC, TSS2_CAR
@ -38,47 +36,34 @@ class RadarInterface(RadarInterfaceBase):
self.rcp = None if CP.radarUnavailable else _create_radar_can_parser(CP.carFingerprint) self.rcp = None if CP.radarUnavailable else _create_radar_can_parser(CP.carFingerprint)
self.trigger_msg = self.RADAR_B_MSGS[-1] self.trigger_msg = self.RADAR_B_MSGS[-1]
self.updated_values = defaultdict(lambda: defaultdict(list)) self.updated_messages = set()
def update(self, can_strings): def update(self, can_strings):
if self.rcp is None: if self.rcp is None:
return None return super().update(None)
addresses = self.rcp.update_strings(can_strings) vls = self.rcp.update_strings(can_strings)
for addr in addresses: self.updated_messages.update(vls)
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_values: if self.trigger_msg not in self.updated_messages:
return None return None
radar_data = self._radar_msg_from_buffer(self.updated_values, self.rcp.can_valid) rr = self._update(self.updated_messages)
self.updated_values.clear() self.updated_messages.clear()
return radar_data return rr
def _radar_msg_from_buffer(self, updated_values, can_valid): def _update(self, updated_messages):
ret = car.RadarData.new_message() ret = car.RadarData.new_message()
errors = [] errors = []
if not can_valid: if not self.rcp.can_valid:
errors.append("canError") errors.append("canError")
ret.errors = errors ret.errors = errors
for ii in sorted(updated_values): for ii in sorted(updated_messages):
if ii not in self.RADAR_A_MSGS: if ii in self.RADAR_A_MSGS:
continue cpt = self.rcp.vl[ii]
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']: if cpt['LONG_DIST'] >= 255 or cpt['NEW_TRACK']:
self.valid_cnt[ii] = 0 # reset counter self.valid_cnt[ii] = 0 # reset counter
if cpt['VALID'] and cpt['LONG_DIST'] < 255: if cpt['VALID'] and cpt['LONG_DIST'] < 255:
@ -86,15 +71,11 @@ class RadarInterface(RadarInterfaceBase):
else: else:
self.valid_cnt[ii] = max(self.valid_cnt[ii] - 1, 0) self.valid_cnt[ii] = max(self.valid_cnt[ii] - 1, 0)
n_b_scores = len(radar_b_msgs['SCORE']) score = self.rcp.vl[ii+16]['SCORE']
if n_b_scores > 0: # print ii, self.valid_cnt[ii], score, cpt['VALID'], cpt['LONG_DIST'], cpt['LAT_DIST']
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 # radar point only valid if it's a valid measurement and score is above 50
if cpt['VALID'] or (score and score > 50 and cpt['LONG_DIST'] < 255 and self.valid_cnt[ii] > 0): 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']: if ii not in self.pts or cpt['NEW_TRACK']:
self.pts[ii] = car.RadarData.RadarPoint.new_message() self.pts[ii] = car.RadarData.RadarPoint.new_message()
self.pts[ii].trackId = self.track_id 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 cereal import messaging, log, car
from common.numpy_fast import interp from common.numpy_fast import interp
from common.params import Params from common.params import Params
from common.realtime import Ratekeeper, Priority, config_realtime_process, DT_MDL from common.realtime import Ratekeeper, Priority, config_realtime_process
from system.swaglog import cloudlog from system.swaglog import cloudlog
from common.kalman.simple_kalman import KF1D from common.kalman.simple_kalman import KF1D
@ -50,8 +50,7 @@ class KalmanParams:
class Track: class Track:
def __init__(self, identifier: int, v_lead: float, kalman_params: KalmanParams): def __init__(self, v_lead: float, kalman_params: KalmanParams):
self.identifier = identifier
self.cnt = 0 self.cnt = 0
self.aLeadTau = _LEAD_ACCEL_TAU self.aLeadTau = _LEAD_ACCEL_TAU
self.K_A = kalman_params.A self.K_A = kalman_params.A
@ -64,12 +63,8 @@ class Track:
self.dRel = d_rel # LONG_DIST self.dRel = d_rel # LONG_DIST
self.yRel = y_rel # -LAT_DIST self.yRel = y_rel # -LAT_DIST
self.vRel = v_rel # REL_SPEED self.vRel = v_rel # REL_SPEED
self.measured = measured # measured or estimate
self.update_vlead(v_lead)
def update_vlead(self, v_lead: float):
self.vLead = v_lead self.vLead = v_lead
self.measured = measured # measured or estimate
# computed velocity and accelerations # computed velocity and accelerations
if self.cnt > 0: if self.cnt > 0:
@ -103,12 +98,11 @@ class Track:
"vLead": float(self.vLead), "vLead": float(self.vLead),
"vLeadK": float(self.vLeadK), "vLeadK": float(self.vLeadK),
"aLeadK": float(self.aLeadK), "aLeadK": float(self.aLeadK),
"aLeadTau": float(self.aLeadTau),
"status": True, "status": True,
"fcw": self.is_potential_fcw(model_prob), "fcw": self.is_potential_fcw(model_prob),
"modelProb": model_prob, "modelProb": model_prob,
"radar": True, "radar": True,
"radarTrackId": self.identifier, "aLeadTau": float(self.aLeadTau)
} }
def potential_low_speed_lead(self, v_ego: float): def potential_low_speed_lead(self, v_ego: float):
@ -164,17 +158,15 @@ def get_RadarState_from_vision(lead_msg: capnp._DynamicStructReader, v_ego: floa
"aLeadTau": 0.3, "aLeadTau": 0.3,
"fcw": False, "fcw": False,
"modelProb": float(lead_msg.prob), "modelProb": float(lead_msg.prob),
"status": True,
"radar": False, "radar": False,
"radarTrackId": -1, "status": True
} }
def get_lead(v_ego: float, ready: bool, tracks: Dict[int, Track], lead_msg: capnp._DynamicStructReader, 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]: model_v_ego: float, low_speed_override: bool = True) -> Dict[str, Any]:
# Determine leads, this is where the essential logic happens # Determine leads, this is where the essential logic happens
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 lead_msg.prob > .5:
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) track = match_vision_to_track(v_ego, lead_msg, tracks)
else: else:
track = None track = None
@ -182,7 +174,7 @@ def get_lead(v_ego: float, ready: bool, tracks: Dict[int, Track], lead_msg: capn
lead_dict = {'status': False} lead_dict = {'status': False}
if track is not None: if track is not None:
lead_dict = track.get_RadarState(lead_msg.prob) lead_dict = track.get_RadarState(lead_msg.prob)
elif track is None and ready and not lead_msg_empty and lead_msg.prob > .5: elif (track is None) and ready and (lead_msg.prob > .5):
lead_dict = get_RadarState_from_vision(lead_msg, v_ego, model_v_ego) lead_dict = get_RadarState_from_vision(lead_msg, v_ego, model_v_ego)
if low_speed_override: if low_speed_override:
@ -212,14 +204,14 @@ class RadarD:
self.ready = False self.ready = False
def update(self, sm: messaging.SubMaster, radar_data: Optional[car.RadarData]): def update(self, sm: messaging.SubMaster, rr: Optional[car.RadarData]):
self.current_time = 1e-9*max(sm.logMonoTime.values()) self.current_time = 1e-9*max(sm.logMonoTime.values())
radar_points = [] radar_points = []
radar_errors = [] radar_errors = []
if radar_data is not None: if rr is not None:
radar_points = radar_data.points radar_points = rr.points
radar_errors = radar_data.errors radar_errors = rr.errors
if sm.updated['carState']: if sm.updated['carState']:
self.v_ego = sm['carState'].vEgo self.v_ego = sm['carState'].vEgo
@ -227,32 +219,26 @@ class RadarD:
if sm.updated['modelV2']: if sm.updated['modelV2']:
self.ready = True self.ready = True
if radar_data is not None: ar_pts = {}
ar_pts = {} for pt in radar_points:
for pt in radar_points: ar_pts[pt.trackId] = [pt.dRel, pt.yRel, pt.vRel, pt.measured]
ar_pts[pt.trackId] = [pt.dRel, pt.yRel, pt.vRel, pt.measured]
# *** remove missing points from meta data *** # *** remove missing points from meta data ***
for ids in list(self.tracks.keys()): for ids in list(self.tracks.keys()):
if ids not in ar_pts: if ids not in ar_pts:
self.tracks.pop(ids, None) self.tracks.pop(ids, None)
# *** compute the tracks *** # *** compute the tracks ***
for ids in ar_pts: for ids in ar_pts:
rpt = ar_pts[ids] rpt = ar_pts[ids]
# align v_ego by a fixed time to align it with the radar measurement # align v_ego by a fixed time to align it with the radar measurement
v_lead = rpt[2] + self.v_ego_hist[0] v_lead = rpt[2] + self.v_ego_hist[0]
# create the track if it doesn't exist or it's a new track # create the track if it doesn't exist or it's a new track
if ids not in self.tracks: if ids not in self.tracks:
self.tracks[ids] = Track(ids, v_lead, self.kalman_params) self.tracks[ids] = Track(v_lead, self.kalman_params)
self.tracks[ids].update(rpt[0], rpt[1], rpt[2], v_lead, rpt[3]) 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 *** # *** publish radarState ***
self.radar_state_valid = sm.all_checks() and len(radar_errors) == 0 self.radar_state_valid = sm.all_checks() and len(radar_errors) == 0
@ -305,33 +291,32 @@ def radard_thread(sm: Optional[messaging.SubMaster] = None, pm: Optional[messagi
cloudlog.info("radard is importing %s", CP.carName) cloudlog.info("radard is importing %s", CP.carName)
RadarInterface = importlib.import_module(f'selfdrive.car.{CP.carName}.radar_interface').RadarInterface RadarInterface = importlib.import_module(f'selfdrive.car.{CP.carName}.radar_interface').RadarInterface
# setup messaging # *** setup messaging
if can_sock is None: if can_sock is None:
can_sock = messaging.sub_sock('can') can_sock = messaging.sub_sock('can')
if sm is None: if sm is None:
sm = messaging.SubMaster(['modelV2', 'carState'], poll=["modelV2"]) sm = messaging.SubMaster(['modelV2', 'carState'], ignore_avg_freq=['modelV2', 'carState']) # Can't check average frequency, since radar determines timing
if pm is None: if pm is None:
pm = messaging.PubMaster(['radarState', 'liveTracks']) pm = messaging.PubMaster(['radarState', 'liveTracks'])
interface = RadarInterface(CP) RI = RadarInterface(CP)
rk = Ratekeeper(1 / DT_MDL, print_delay_threshold=None) rk = Ratekeeper(1.0 / CP.radarTimeStep, print_delay_threshold=None)
radar = RadarD(DT_MDL, interface.delay) RD = RadarD(CP.radarTimeStep, RI.delay)
while True: while 1:
sm.update() can_strings = messaging.drain_sock_raw(can_sock, wait_for_one=True)
rr = RI.update(can_strings)
if sm.updated['modelV2']: if rr is None:
can_strings = messaging.drain_sock_raw(can_sock) continue
if len(can_strings) == 0:
radar_data = None sm.update(0)
else:
radar_data = interface.update(can_strings)
radar.update(sm, radar_data) RD.update(sm, rr)
radar.publish(pm, -rk.remaining*1000.0) 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): 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"], subs=["radarState", "liveTracks"],
ignore=["logMonoTime", "valid", "radarState.cumLagMs"], ignore=["logMonoTime", "valid", "radarState.cumLagMs"],
init_callback=get_car_params_callback, init_callback=get_car_params_callback,
should_recv_callback=MessageBasedRcvCallback("modelV2"), should_recv_callback=MessageBasedRcvCallback("can"),
unlocked_pubs=["can"], main_pub="can",
), ),
ProcessConfig( ProcessConfig(
proc_name="plannerd", proc_name="plannerd",

2
selfdrive/test/process_replay/ref_commit
Unescape View File

@ -1 +1 @@
ac3f87bad519df408a295d2e007e0de58b11690c 72e3d7b660ee92f5adcc249112cf04c703f4bf9e
Write Preview
Loading…
Cancel
Save