card parses radar points (#33443)

* interfaces returns radarinterface

old-commit-hash: 9ad1f096bfca3712320f19b1b49aa9e6ac9b68e4

* bump

old-commit-hash: 20334a8b257c6037e11d02f2ba7b1f02c59f3240

* get RI from opendbc

old-commit-hash: b5f6d0c48c90927926e9dd557130075aeec5edee

* stash so far

old-commit-hash: 5aa2c842eb152316434c17a661df05bb8af61f47

* new liveTracks message (radard expects and needs RadarData)

* this should just work?

* whoops

* fix that

* rm liveTracks from radard pm

* fix proceess replay

* lol fcw diff, something's not right

* actually there's fcw in original route. it's pretty close

* no tracks!

* fix test_leads

* CPU moved across procs

* fix not engageable from onroadEvents

* bump

* fixes

* bump to master

* radard publishes w/ modelV2 now, so it will always be sent. check valid which radard sets using liveTracks avg freq

* fix that (it works!)

* combine

join

* bump

* bump

* deprecate

* why

* fix incorrect args

* remove cumLagMs from process_replay

* update refs
This commit is contained in:
Shane Smiskol 2024-09-05 22:40:17 -07:00 committed by GitHub
parent aef650013e
commit 922348f33d
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
13 changed files with 95 additions and 75 deletions

View File

@ -508,6 +508,7 @@ struct CarParams {
carFw @44 :List(CarFw);
radarTimeStep @45: Float32 = 0.05; # time delta between radar updates, 20Hz is very standard
radarDelay @74 :Float32;
fingerprintSource @49: FingerprintSource;
networkLocation @50 :NetworkLocation; # Where Panda/C2 is integrated into the car's CAN network

View File

@ -611,7 +611,6 @@ struct RadarState @0x9a185389d6fdd05f {
leadOne @3 :LeadData;
leadTwo @4 :LeadData;
cumLagMs @5 :Float32;
struct LeadData {
dRel @0 :Float32;
@ -641,6 +640,7 @@ struct RadarState @0x9a185389d6fdd05f {
calCycleDEPRECATED @8 :Int32;
calPercDEPRECATED @9 :Int8;
canMonoTimesDEPRECATED @10 :List(UInt64);
cumLagMsDEPRECATED @5 :Float32;
}
struct LiveCalibrationData {
@ -671,7 +671,7 @@ struct LiveCalibrationData {
}
}
struct LiveTracks {
struct LiveTracksDEPRECATED {
trackId @0 :Int32;
dRel @1 :Float32;
yRel @2 :Float32;
@ -2335,7 +2335,7 @@ struct Event {
pandaStates @81 :List(PandaState);
peripheralState @80 :PeripheralState;
radarState @13 :RadarState;
liveTracks @16 :List(LiveTracks);
liveTracks @131 :Car.RadarData;
sendcan @17 :List(CanData);
liveCalibration @19 :LiveCalibrationData;
carState @22 :Car.CarState;
@ -2465,5 +2465,6 @@ struct Event {
navModelDEPRECATED @104 :NavModelData;
uiPlanDEPRECATED @106 :UiPlan;
liveLocationKalmanDEPRECATED @72 :LiveLocationKalman;
liveTracksDEPRECATED @16 :List(LiveTracksDEPRECATED);
}
}

@ -1 +1 @@
Subproject commit 86be858a37217d003e8d8b0097522656df7a5870
Subproject commit c0a9ab5c39197d48cc920a818a85610e526f9e73

View File

@ -16,8 +16,8 @@ from openpilot.common.swaglog import cloudlog, ForwardingHandler
from opendbc.car import DT_CTRL, carlog, structs
from opendbc.car.can_definitions import CanData, CanRecvCallable, CanSendCallable
from opendbc.car.fw_versions import ObdCallback
from opendbc.car.car_helpers import get_car
from opendbc.car.interfaces import CarInterfaceBase
from opendbc.car.car_helpers import get_car, get_radar_interface
from opendbc.car.interfaces import CarInterfaceBase, RadarInterfaceBase
from openpilot.selfdrive.pandad import can_capnp_to_list, can_list_to_can_capnp
from openpilot.selfdrive.car.cruise import VCruiseHelper
from openpilot.selfdrive.car.car_specific import CarSpecificEvents, MockCarState
@ -63,13 +63,14 @@ def can_comm_callbacks(logcan: messaging.SubSocket, sendcan: messaging.PubSocket
class Car:
CI: CarInterfaceBase
RI: RadarInterfaceBase
CP: structs.CarParams
CP_capnp: car.CarParams
def __init__(self, CI=None) -> None:
def __init__(self, CI=None, RI=None) -> None:
self.can_sock = messaging.sub_sock('can', timeout=20)
self.sm = messaging.SubMaster(['pandaStates', 'carControl', 'onroadEvents'])
self.pm = messaging.PubMaster(['sendcan', 'carState', 'carParams', 'carOutput'])
self.pm = messaging.PubMaster(['sendcan', 'carState', 'carParams', 'carOutput', 'liveTracks'])
self.can_rcv_cum_timeout_counter = 0
@ -101,12 +102,14 @@ class Car:
cached_params = structs.CarParams(carName=_cached_params.carName, carFw=_cached_params.carFw, carVin=_cached_params.carVin)
self.CI = get_car(*self.can_callbacks, obd_callback(self.params), experimental_long_allowed, num_pandas, cached_params)
self.RI = get_radar_interface(self.CI.CP)
self.CP = self.CI.CP
# continue onto next fingerprinting step in pandad
self.params.put_bool("FirmwareQueryDone", True)
else:
self.CI, self.CP = CI, CI.CP
self.RI = RI
# set alternative experiences from parameters
self.disengage_on_accelerator = self.params.get_bool("DisengageOnAccelerator")
@ -149,7 +152,7 @@ class Car:
# card is driven by can recv, expected at 100Hz
self.rk = Ratekeeper(100, print_delay_threshold=None)
def state_update(self) -> car.CarState:
def state_update(self) -> tuple[car.CarState, structs.RadarData | None]:
"""carState update loop, driven by can"""
# Update carState from CAN
@ -159,6 +162,9 @@ class Car:
if self.CP.carName == 'mock':
CS = self.mock_carstate.update(CS)
# Update radar tracks from CAN
RD: structs.RadarData | None = self.RI.update(can_capnp_to_list(can_strs))
self.sm.update(0)
can_rcv_valid = len(can_strs) > 0
@ -175,9 +181,9 @@ class Car:
CS.vCruise = float(self.v_cruise_helper.v_cruise_kph)
CS.vCruiseCluster = float(self.v_cruise_helper.v_cruise_cluster_kph)
return CS
return CS, RD
def update_events(self, CS: car.CarState):
def update_events(self, CS: car.CarState, RD: structs.RadarData | None):
self.events.clear()
CS.events = self.car_events.update(self.CI.CS, self.CS_prev, self.CI.CC, self.CC_prev).to_msg()
@ -196,9 +202,12 @@ class Car:
(CS.regenBraking and (not self.CS_prev.regenBraking or not CS.standstill)):
self.events.add(EventName.pedalPressed)
if RD is not None and len(RD.errors):
self.events.add(EventName.radarFault)
CS.events = self.events.to_msg()
def state_publish(self, CS: car.CarState):
def state_publish(self, CS: car.CarState, RD: structs.RadarData | None):
"""carState and carParams publish loop"""
# carParams - logged every 50 seconds (> 1 per segment)
@ -222,6 +231,12 @@ class Car:
cs_send.carState.cumLagMs = -self.rk.remaining * 1000.
self.pm.send('carState', cs_send)
if RD is not None:
tracks_msg = messaging.new_message('liveTracks')
tracks_msg.valid = CS.canValid and len(RD.errors) == 0
tracks_msg.liveTracks = convert_to_capnp(RD)
self.pm.send('liveTracks', tracks_msg)
def controls_update(self, CS: car.CarState, CC: car.CarControl):
"""control update loop, driven by carControl"""
@ -241,14 +256,14 @@ class Car:
self.CC_prev = CC
def step(self):
CS = self.state_update()
CS, RD = self.state_update()
self.update_events(CS)
self.update_events(CS, RD)
if not self.sm['carControl'].enabled and self.events.contains(ET.ENABLE):
self.v_cruise_helper.initialize_v_cruise(CS, self.experimental_mode)
self.state_publish(CS)
self.state_publish(CS, RD)
initialized = (not any(e.name == EventName.controlsInitializing for e in self.sm['onroadEvents']) and
self.sm.seen['onroadEvents'])

View File

@ -35,7 +35,7 @@ def asdictref(obj) -> dict[str, Any]:
return _asdictref_inner(obj)
def convert_to_capnp(struct: structs.CarParams | structs.CarState | structs.CarControl.Actuators) -> capnp.lib.capnp._DynamicStructBuilder:
def convert_to_capnp(struct: structs.CarParams | structs.CarState | structs.CarControl.Actuators | structs.RadarData) -> capnp.lib.capnp._DynamicStructBuilder:
struct_dict = asdictref(struct)
if isinstance(struct, structs.CarParams):
@ -51,6 +51,8 @@ def convert_to_capnp(struct: structs.CarParams | structs.CarState | structs.CarC
struct_capnp = car.CarState.new_message(**struct_dict)
elif isinstance(struct, structs.CarControl.Actuators):
struct_capnp = car.CarControl.Actuators.new_message(**struct_dict)
elif isinstance(struct, structs.RadarData):
struct_capnp = car.RadarData.new_message(**struct_dict)
else:
raise ValueError(f"Unsupported struct type: {type(struct)}")

View File

@ -448,7 +448,7 @@ class TestCarModelBase(unittest.TestCase):
checks['cruiseState'] += CS.cruiseState.enabled != self.safety.get_cruise_engaged_prev()
else:
# Check for enable events on rising edge of controls allowed
card.update_events(CS)
card.update_events(CS, None)
card.CS_prev = CS
button_enable = (any(evt.enable for evt in CS.events) and
not any(evt == EventName.pedalPressed for evt in card.events.names))

View File

@ -96,7 +96,7 @@ class Controls:
'carOutput', 'driverMonitoringState', 'longitudinalPlan', 'livePose',
'managerState', 'liveParameters', 'radarState', 'liveTorqueParameters',
'testJoystick'] + self.camera_packets + self.sensor_packets + self.gps_packets,
ignore_alive=ignore, ignore_avg_freq=ignore+['radarState', 'testJoystick'], ignore_valid=['testJoystick', ],
ignore_alive=ignore, ignore_avg_freq=ignore+['testJoystick'], ignore_valid=['testJoystick', ],
frequency=int(1/DT_CTRL))
self.joystick_mode = self.params.get_bool("JoystickDebugMode")
@ -301,7 +301,7 @@ class Controls:
self.events.add(EventName.cameraFrameRate)
if not REPLAY and self.rk.lagging:
self.events.add(EventName.controlsdLagging)
if len(self.sm['radarState'].radarErrors) or ((not self.rk.lagging or REPLAY) and not self.sm.all_checks(['radarState'])):
if not self.sm.valid['radarState']:
self.events.add(EventName.radarFault)
if not self.sm.valid['pandaStates']:
self.events.add(EventName.usbError)

View File

@ -1,18 +1,15 @@
#!/usr/bin/env python3
import importlib
import math
from collections import deque
from typing import Any
import capnp
from cereal import messaging, log, car
from opendbc.car import structs
from openpilot.common.numpy_fast import interp
from openpilot.common.params import Params
from openpilot.common.realtime import DT_CTRL, Ratekeeper, Priority, config_realtime_process
from openpilot.common.realtime import DT_MDL, Priority, config_realtime_process
from openpilot.common.swaglog import cloudlog
from openpilot.common.simple_kalman import KF1D
from openpilot.selfdrive.pandad import can_capnp_to_list
# Default lead acceleration decay set to 50% at 1s
@ -194,14 +191,14 @@ def get_lead(v_ego: float, ready: bool, tracks: dict[int, Track], lead_msg: capn
class RadarD:
def __init__(self, radar_ts: float, delay: int = 0):
def __init__(self, delay: float = 0.0):
self.current_time = 0.0
self.tracks: dict[int, Track] = {}
self.kalman_params = KalmanParams(radar_ts)
self.kalman_params = KalmanParams(DT_MDL)
self.v_ego = 0.0
self.v_ego_hist = deque([0.0], maxlen=delay+1)
self.v_ego_hist = deque([0.0], maxlen=int(round(delay / DT_MDL))+1)
self.last_v_ego_frame = -1
self.radar_state: capnp._DynamicStructBuilder | None = None
@ -209,7 +206,7 @@ class RadarD:
self.ready = False
def update(self, sm: messaging.SubMaster, rr: structs.RadarData):
def update(self, sm: messaging.SubMaster, rr: car.RadarData):
self.ready = sm.seen['modelV2']
self.current_time = 1e-9*max(sm.logMonoTime.values())
@ -255,27 +252,14 @@ class RadarD:
self.radar_state.leadOne = get_lead(self.v_ego, self.ready, self.tracks, leads_v3[0], model_v_ego, low_speed_override=True)
self.radar_state.leadTwo = get_lead(self.v_ego, self.ready, self.tracks, leads_v3[1], model_v_ego, low_speed_override=False)
def publish(self, pm: messaging.PubMaster, lag_ms: float):
def publish(self, pm: messaging.PubMaster):
assert self.radar_state is not None
radar_msg = messaging.new_message("radarState")
radar_msg.valid = self.radar_state_valid
radar_msg.radarState = self.radar_state
radar_msg.radarState.cumLagMs = lag_ms
pm.send("radarState", radar_msg)
# publish tracks for UI debugging (keep last)
tracks_msg = messaging.new_message('liveTracks', len(self.tracks))
tracks_msg.valid = self.radar_state_valid
for index, tid in enumerate(sorted(self.tracks.keys())):
tracks_msg.liveTracks[index] = {
"trackId": tid,
"dRel": float(self.tracks[tid].dRel),
"yRel": float(self.tracks[tid].yRel),
"vRel": float(self.tracks[tid].vRel),
}
pm.send('liveTracks', tracks_msg)
# fuses camera and radar data for best lead detection
def main() -> None:
@ -286,31 +270,17 @@ def main() -> None:
CP = messaging.log_from_bytes(Params().get("CarParams", block=True), car.CarParams)
cloudlog.info("radard got CarParams")
# import the radar from the fingerprint
cloudlog.info("radard is importing %s", CP.carName)
RadarInterface = importlib.import_module(f'opendbc.car.{CP.carName}.radar_interface').RadarInterface
# *** setup messaging
can_sock = messaging.sub_sock('can')
sm = messaging.SubMaster(['modelV2', 'carState'], frequency=int(1./DT_CTRL))
pm = messaging.PubMaster(['radarState', 'liveTracks'])
sm = messaging.SubMaster(['modelV2', 'carState', 'liveTracks'], poll='modelV2')
pm = messaging.PubMaster(['radarState'])
RI = RadarInterface(CP)
rk = Ratekeeper(1.0 / CP.radarTimeStep, print_delay_threshold=None)
RD = RadarD(CP.radarTimeStep, RI.delay)
RD = RadarD(CP.radarDelay)
while 1:
can_strings = messaging.drain_sock_raw(can_sock, wait_for_one=True)
rr: structs.RadarData | None = RI.update(can_capnp_to_list(can_strings))
sm.update(0)
if rr is None:
continue
sm.update()
RD.update(sm, rr)
RD.publish(pm, -rk.remaining*1000.0)
rk.monitor_time()
RD.update(sm, sm['liveTracks'])
RD.publish(pm)
if __name__ == "__main__":

View File

@ -11,19 +11,21 @@ class TestLeads:
def single_iter_pkg():
# single iter package, with meaningless cans and empty carState/modelV2
msgs = []
for _ in range(5):
for _ in range(500):
can = messaging.new_message("can", 1)
cs = messaging.new_message("carState")
cp = messaging.new_message("carParams")
msgs.append(can.as_reader())
msgs.append(cs.as_reader())
msgs.append(cp.as_reader())
model = messaging.new_message("modelV2")
msgs.append(model.as_reader())
return msgs
msgs = [m for _ in range(3) for m in single_iter_pkg()]
out = replay_process_with_name("radard", msgs, fingerprint=TOYOTA.TOYOTA_COROLLA_TSS2)
states = [m for m in out if m.which() == "radarState"]
failures = [not state.valid and len(state.radarState.radarErrors) for state in states]
out = replay_process_with_name("card", msgs, fingerprint=TOYOTA.TOYOTA_COROLLA_TSS2)
states = [m for m in out if m.which() == "liveTracks"]
failures = [not state.valid and len(state.liveTracks.errors) for state in states]
assert len(states) == 0 or all(failures)

View File

@ -1,6 +1,6 @@
from collections import defaultdict
from cereal import messaging
from cereal import messaging, car
from opendbc.car.fingerprints import MIGRATION
from opendbc.car.toyota.values import EPS_SCALE
from openpilot.selfdrive.test.process_replay.vision_meta import meta_from_encode_index
@ -17,6 +17,7 @@ def migrate_all(lr, manager_states=False, panda_states=False, camera_states=Fals
msgs = migrate_carOutput(msgs)
msgs = migrate_controlsState(msgs)
msgs = migrate_liveLocationKalman(msgs)
msgs = migrate_liveTracks(msgs)
if manager_states:
msgs = migrate_managerState(msgs)
if panda_states:
@ -28,6 +29,35 @@ def migrate_all(lr, manager_states=False, panda_states=False, camera_states=Fals
return msgs
def migrate_liveTracks(lr):
all_msgs = []
for msg in lr:
if msg.which() != "liveTracksDEPRECATED":
all_msgs.append(msg)
continue
new_msg = messaging.new_message('liveTracks')
new_msg.valid = msg.valid
new_msg.logMonoTime = msg.logMonoTime
pts = []
for track in msg.liveTracksDEPRECATED:
pt = car.RadarData.RadarPoint()
pt.trackId = track.trackId
pt.dRel = track.dRel
pt.yRel = track.yRel
pt.vRel = track.vRel
pt.aRel = track.aRel
pt.measured = True
pts.append(pt)
new_msg.liveTracks.points = pts
all_msgs.append(new_msg.as_reader())
return all_msgs
def migrate_liveLocationKalman(lr):
# migration needed only for routes before livePose
if any(msg.which() == 'livePose' for msg in lr):

View File

@ -480,7 +480,7 @@ CONFIGS = [
ProcessConfig(
proc_name="card",
pubs=["pandaStates", "carControl", "onroadEvents", "can"],
subs=["sendcan", "carState", "carParams", "carOutput"],
subs=["sendcan", "carState", "carParams", "carOutput", "liveTracks"],
ignore=["logMonoTime", "carState.cumLagMs"],
init_callback=card_fingerprint_callback,
should_recv_callback=card_rcv_callback,
@ -490,12 +490,11 @@ CONFIGS = [
),
ProcessConfig(
proc_name="radard",
pubs=["can", "carState", "modelV2"],
subs=["radarState", "liveTracks"],
ignore=["logMonoTime", "radarState.cumLagMs"],
pubs=["liveTracks", "carState", "modelV2"],
subs=["radarState"],
ignore=["logMonoTime"],
init_callback=get_car_params_callback,
should_recv_callback=MessageBasedRcvCallback("can"),
main_pub="can",
should_recv_callback=FrequencyBasedRcvCallback("modelV2"),
),
ProcessConfig(
proc_name="plannerd",

View File

@ -1 +1 @@
fb0827a00b21b97733f8385cf7ce87a21a526f4c
2b842a259e15f4b8679f16a14de405b5df06ae1f

View File

@ -36,7 +36,7 @@ MAX_TOTAL_CPU = 260. # total for all 8 cores
PROCS = {
# Baseline CPU usage by process
"selfdrive.controls.controlsd": 32.0,
"selfdrive.car.card": 26.0,
"selfdrive.car.card": 31.0,
"./loggerd": 14.0,
"./encoderd": 17.0,
"./camerad": 14.5,
@ -44,7 +44,7 @@ PROCS = {
"./ui": 18.0,
"selfdrive.locationd.paramsd": 9.0,
"./sensord": 7.0,
"selfdrive.controls.radard": 7.0,
"selfdrive.controls.radard": 2.0,
"selfdrive.modeld.modeld": 13.0,
"selfdrive.modeld.dmonitoringmodeld": 8.0,
"system.hardware.hardwared": 3.87,