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
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. 37
      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. 67
      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 None
return super().update(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 None
return super().update(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 None
return super().update(None)
vls = self.rcp.update_strings(can_strings)
self.updated_messages.update(vls)

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

@ -1,6 +1,4 @@
#!/usr/bin/env python3
from collections import defaultdict
from cereal import car
from opendbc.can.parser import CANParser
from selfdrive.car.interfaces import RadarInterfaceBase
@ -30,40 +28,29 @@ class RadarInterface(RadarInterfaceBase):
else:
self.rcp = _create_nidec_can_parser(CP.carFingerprint)
self.trigger_msg = 0x445
self.updated_values = defaultdict(lambda: defaultdict(list))
self.updated_messages = set()
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 None
return super().update(None)
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)
vls = self.rcp.update_strings(can_strings)
self.updated_messages.update(vls)
if self.trigger_msg not in self.updated_values:
if self.trigger_msg not in self.updated_messages:
return None
radar_data = self._radar_msg_from_buffer(self.updated_values, self.rcp.can_valid)
self.updated_values.clear()
rr = self._update(self.updated_messages)
self.updated_messages.clear()
return rr
return radar_data
def _radar_msg_from_buffer(self, updated_values, can_valid):
def _update(self, updated_messages):
ret = car.RadarData.new_message()
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:
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
@ -84,7 +71,7 @@ class RadarInterface(RadarInterfaceBase):
del self.pts[ii]
errors = []
if not can_valid:
if not self.rcp.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 None
return super().update(None)
vls = self.rcp.update_strings(can_strings)
self.updated_messages.update(vls)

8
selfdrive/car/interfaces.py
Unescape View File

@ -1,5 +1,6 @@
import yaml
import os
import time
from abc import abstractmethod, ABC
from typing import Any, Dict, Optional, Tuple, List, Callable
@ -309,9 +310,14 @@ 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):
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):

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 None
return super().update(None)
values = self.rcp.update_strings(can_strings)
self.updated_messages.update(values)

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

@ -1,6 +1,4 @@
#!/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
@ -38,47 +36,34 @@ 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_values = defaultdict(lambda: defaultdict(list))
self.updated_messages = set()
def update(self, can_strings):
if self.rcp is None:
return None
return super().update(None)
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)
vls = self.rcp.update_strings(can_strings)
self.updated_messages.update(vls)
if self.trigger_msg not in self.updated_values:
if self.trigger_msg not in self.updated_messages:
return None
radar_data = self._radar_msg_from_buffer(self.updated_values, self.rcp.can_valid)
self.updated_values.clear()
rr = self._update(self.updated_messages)
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()
errors = []
if not can_valid:
if not self.rcp.can_valid:
errors.append("canError")
ret.errors = errors
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]
for ii in sorted(updated_messages):
if ii in self.RADAR_A_MSGS:
cpt = self.rcp.vl[ii]
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:
@ -86,15 +71,11 @@ class RadarInterface(RadarInterfaceBase):
else:
self.valid_cnt[ii] = max(self.valid_cnt[ii] - 1, 0)
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
score = self.rcp.vl[ii+16]['SCORE']
# print ii, self.valid_cnt[ii], score, cpt['VALID'], cpt['LONG_DIST'], cpt['LAT_DIST']
# 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']:
self.pts[ii] = car.RadarData.RadarPoint.new_message()
self.pts[ii].trackId = self.track_id

67
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, DT_MDL
from common.realtime import Ratekeeper, Priority, config_realtime_process
from system.swaglog import cloudlog
from common.kalman.simple_kalman import KF1D
@ -50,8 +50,7 @@ class KalmanParams:
class Track:
def __init__(self, identifier: int, v_lead: float, kalman_params: KalmanParams):
self.identifier = identifier
def __init__(self, v_lead: float, kalman_params: KalmanParams):
self.cnt = 0
self.aLeadTau = _LEAD_ACCEL_TAU
self.K_A = kalman_params.A
@ -64,12 +63,8 @@ class Track:
self.dRel = d_rel # LONG_DIST
self.yRel = y_rel # -LAT_DIST
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.measured = measured # measured or estimate
# computed velocity and accelerations
if self.cnt > 0:
@ -103,12 +98,11 @@ 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,
"radarTrackId": self.identifier,
"aLeadTau": float(self.aLeadTau)
}
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,
"fcw": False,
"modelProb": float(lead_msg.prob),
"status": True,
"radar": False,
"radarTrackId": -1,
"status": True
}
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
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:
if len(tracks) > 0 and ready and lead_msg.prob > .5:
track = match_vision_to_track(v_ego, lead_msg, tracks)
else:
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}
if track is not None:
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)
if low_speed_override:
@ -212,14 +204,14 @@ class RadarD:
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())
radar_points = []
radar_errors = []
if radar_data is not None:
radar_points = radar_data.points
radar_errors = radar_data.errors
if rr is not None:
radar_points = rr.points
radar_errors = rr.errors
if sm.updated['carState']:
self.v_ego = sm['carState'].vEgo
@ -227,7 +219,6 @@ class RadarD:
if sm.updated['modelV2']:
self.ready = True
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]
@ -246,13 +237,8 @@ class RadarD:
# 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] = Track(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
@ -305,31 +291,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'], 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:
pm = messaging.PubMaster(['radarState', 'liveTracks'])
interface = RadarInterface(CP)
RI = RadarInterface(CP)
rk = Ratekeeper(1 / DT_MDL, print_delay_threshold=None)
radar = RadarD(DT_MDL, interface.delay)
rk = Ratekeeper(1.0 / CP.radarTimeStep, print_delay_threshold=None)
RD = RadarD(CP.radarTimeStep, RI.delay)
while True:
sm.update()
while 1:
can_strings = messaging.drain_sock_raw(can_sock, wait_for_one=True)
rr = RI.update(can_strings)
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)
if rr is None:
continue
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()

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("modelV2"),
unlocked_pubs=["can"],
should_recv_callback=MessageBasedRcvCallback("can"),
main_pub="can",
),
ProcessConfig(
proc_name="plannerd",

2
selfdrive/test/process_replay/ref_commit
Unescape View File

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