mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-11-07 21:13:41 +08:00
firestar5683
430 lines
17 KiB
Python
Executable File
430 lines
17 KiB
Python
Executable File
#!/usr/bin/env python3
|
|
import math
|
|
import numpy as np
|
|
from collections import deque
|
|
from typing import Any
|
|
import os
|
|
import secrets
|
|
import time
|
|
|
|
import capnp
|
|
from openpilot.cereal import messaging, log
|
|
from opendbc.car.structs import car
|
|
from openpilot.common.filter_simple import FirstOrderFilter
|
|
from openpilot.common.params import Params
|
|
from openpilot.common.realtime import DT_MDL, Priority, config_realtime_process, set_core_affinity
|
|
from openpilot.common.swaglog import cloudlog
|
|
from openpilot.common.simple_kalman import KF1D
|
|
from openpilot.starpilot.conditional_mode.adjacent import (
|
|
CLOCK_PAIR_MAX_SKEW_NS, MODEL_EOF_MAX_AGE_NS, MODEL_MAX_AGE_NS, SOURCE_MAX_AGE_NS,
|
|
AdjacentFrame, AdjacentObservation, AdjacentTrack, MonoSource, UNKNOWN, evaluate,
|
|
)
|
|
from openpilot.starpilot.feature_runtime import enabled as feature_enabled
|
|
from openpilot.starpilot.longitudinal.voacc import selected as voacc_selected
|
|
|
|
|
|
# Default lead acceleration decay set to 50% at 1s
|
|
_LEAD_ACCEL_TAU = 1.5
|
|
|
|
# radar tracks
|
|
SPEED, ACCEL = 0, 1 # Kalman filter states enum
|
|
|
|
# stationary qualification parameters
|
|
V_EGO_STATIONARY = 4. # no stationary object flag below this speed
|
|
|
|
RADAR_TO_CAMERA = 1.52 # RADAR is ~ 1.5m ahead from center of mesh frame
|
|
|
|
|
|
def _clock_pair() -> tuple[int, int, int] | None:
|
|
"""Return a bounded MONOTONIC/BOOTTIME pair; missing BOOTTIME stays unknown."""
|
|
boot_clock = getattr(time, 'CLOCK_BOOTTIME', None)
|
|
if boot_clock is None:
|
|
return None
|
|
try:
|
|
before = time.monotonic_ns()
|
|
boot = time.clock_gettime_ns(boot_clock)
|
|
after = time.monotonic_ns()
|
|
except OSError:
|
|
return None
|
|
return (before + after) // 2, boot, after - before
|
|
|
|
|
|
class KalmanParams:
|
|
def __init__(self, dt: float):
|
|
# Lead Kalman Filter params, calculating K from A, C, Q, R requires the control library.
|
|
# hardcoding a lookup table to compute K for values of radar_ts between 0.01s and 0.2s
|
|
assert dt > .01 and dt < .2, "Radar time step must be between .01s and 0.2s"
|
|
self.A = [[1.0, dt], [0.0, 1.0]]
|
|
self.C = [1.0, 0.0]
|
|
#Q = np.matrix([[10., 0.0], [0.0, 100.]])
|
|
#R = 1e3
|
|
#K = np.matrix([[ 0.05705578], [ 0.03073241]])
|
|
dts = [i * 0.01 for i in range(1, 21)]
|
|
K0 = [0.12287673, 0.14556536, 0.16522756, 0.18281627, 0.1988689, 0.21372394,
|
|
0.22761098, 0.24069424, 0.253096, 0.26491023, 0.27621103, 0.28705801,
|
|
0.29750003, 0.30757767, 0.31732515, 0.32677158, 0.33594201, 0.34485814,
|
|
0.35353899, 0.36200124]
|
|
K1 = [0.29666309, 0.29330885, 0.29042818, 0.28787125, 0.28555364, 0.28342219,
|
|
0.28144091, 0.27958406, 0.27783249, 0.27617149, 0.27458948, 0.27307714,
|
|
0.27162685, 0.27023228, 0.26888809, 0.26758976, 0.26633338, 0.26511557,
|
|
0.26393339, 0.26278425]
|
|
self.K = [[np.interp(dt, dts, K0)], [np.interp(dt, dts, K1)]]
|
|
|
|
|
|
class Track:
|
|
def __init__(self, identifier: int, v_lead: float, kalman_params: KalmanParams):
|
|
self.identifier = identifier
|
|
self.cnt = 0
|
|
self.aLeadTau = FirstOrderFilter(_LEAD_ACCEL_TAU, 0.45, DT_MDL)
|
|
self.K_A = kalman_params.A
|
|
self.K_C = kalman_params.C
|
|
self.K_K = kalman_params.K
|
|
self.kf = KF1D([[v_lead], [0.0]], self.K_A, self.K_C, self.K_K)
|
|
|
|
def update(self, d_rel: float, y_rel: float, v_rel: float, v_lead: float):
|
|
# relative values, copy
|
|
self.dRel = d_rel # LONG_DIST
|
|
self.yRel = y_rel # -LAT_DIST
|
|
self.vRel = v_rel # REL_SPEED
|
|
self.vLead = v_lead
|
|
|
|
# computed velocity and accelerations
|
|
if self.cnt > 0:
|
|
self.kf.update(self.vLead)
|
|
|
|
self.vLeadK = float(self.kf.x[SPEED][0])
|
|
self.aLeadK = float(self.kf.x[ACCEL][0])
|
|
|
|
# Learn if constant acceleration
|
|
if abs(self.aLeadK) < 0.5:
|
|
self.aLeadTau.x = _LEAD_ACCEL_TAU
|
|
else:
|
|
self.aLeadTau.update(0.0)
|
|
|
|
self.cnt += 1
|
|
|
|
def get_RadarState(self, model_prob: float = 0.0):
|
|
return {
|
|
"dRel": float(self.dRel),
|
|
"yRel": float(self.yRel),
|
|
"vRel": float(self.vRel),
|
|
"vLead": float(self.vLead),
|
|
"vLeadK": float(self.vLeadK),
|
|
"aLeadK": float(self.aLeadK),
|
|
"aLeadTau": float(self.aLeadTau.x),
|
|
"present": True,
|
|
"modelProb": model_prob,
|
|
"radar": True,
|
|
"radarTrackId": self.identifier,
|
|
}
|
|
|
|
def potential_low_speed_lead(self, v_ego: float):
|
|
# stop for stuff in front of you and low speed, even without model confirmation
|
|
# Radar points closer than 0.75, are almost always glitches on toyota radars
|
|
return abs(self.yRel) < 1.0 and (v_ego < V_EGO_STATIONARY) and (0.75 < self.dRel < 25)
|
|
|
|
def __str__(self):
|
|
ret = f"x: {self.dRel:4.1f} y: {self.yRel:4.1f} v: {self.vRel:4.1f} a: {self.aLeadK:4.1f}"
|
|
return ret
|
|
|
|
|
|
def laplacian_pdf(x: float, mu: float, b: float):
|
|
b = max(b, 1e-4)
|
|
return math.exp(-abs(x-mu)/b)
|
|
|
|
|
|
def match_vision_to_track(v_ego: float, lead: capnp._DynamicStructReader, tracks: dict[int, Track]):
|
|
offset_vision_dist = lead.x[0] - RADAR_TO_CAMERA
|
|
|
|
def prob(c):
|
|
prob_d = laplacian_pdf(c.dRel, offset_vision_dist, lead.xStd[0])
|
|
prob_y = laplacian_pdf(c.yRel, -lead.y[0], lead.yStd[0])
|
|
prob_v = laplacian_pdf(c.vRel + v_ego, lead.v[0], lead.vStd[0])
|
|
|
|
# This isn't exactly right, but it's a good heuristic
|
|
return prob_d * prob_y * prob_v
|
|
|
|
track = max(tracks.values(), key=prob)
|
|
|
|
# if no 'sane' match is found return -1
|
|
# stationary radar points can be false positives
|
|
dist_sane = abs(track.dRel - offset_vision_dist) < max([(offset_vision_dist)*.25, 5.0])
|
|
vel_sane = (abs(track.vRel + v_ego - lead.v[0]) < 10) or (v_ego + track.vRel > 3)
|
|
if dist_sane and vel_sane:
|
|
return track
|
|
else:
|
|
return None
|
|
|
|
|
|
def get_RadarState_from_vision(lead_msg: capnp._DynamicStructReader, v_ego: float, model_v_ego: float, lead_prob: float):
|
|
lead_v_rel_pred = lead_msg.v[0] - model_v_ego
|
|
return {
|
|
"dRel": float(lead_msg.x[0] - RADAR_TO_CAMERA),
|
|
"yRel": float(-lead_msg.y[0]),
|
|
"vRel": float(lead_v_rel_pred),
|
|
"vLead": float(v_ego + lead_v_rel_pred),
|
|
"vLeadK": float(v_ego + lead_v_rel_pred),
|
|
"aLeadK": float(lead_msg.a[0]),
|
|
"aLeadTau": 0.3,
|
|
"modelProb": float(lead_prob),
|
|
"present": True,
|
|
"radar": False,
|
|
"radarTrackId": -1,
|
|
}
|
|
|
|
|
|
def get_lead(v_ego: float, ready: bool, tracks: dict[int, Track], lead_msg: capnp._DynamicStructReader,
|
|
model_v_ego: float, lead_prob: 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_prob > .5:
|
|
track = match_vision_to_track(v_ego, lead_msg, tracks)
|
|
else:
|
|
track = None
|
|
|
|
lead_dict = {'present': False}
|
|
if track is not None:
|
|
lead_dict = track.get_RadarState(lead_prob)
|
|
elif (track is None) and ready and (lead_prob > .5):
|
|
lead_dict = get_RadarState_from_vision(lead_msg, v_ego, model_v_ego, lead_prob)
|
|
|
|
if low_speed_override:
|
|
low_speed_tracks = [c for c in tracks.values() if c.potential_low_speed_lead(v_ego)]
|
|
if len(low_speed_tracks) > 0:
|
|
closest_track = min(low_speed_tracks, key=lambda c: c.dRel)
|
|
|
|
# Only choose new track if it is actually closer than the previous one
|
|
if (not lead_dict['present']) or (closest_track.dRel < lead_dict['dRel']):
|
|
lead_dict = closest_track.get_RadarState()
|
|
|
|
return lead_dict
|
|
|
|
|
|
class RadarD:
|
|
def __init__(self, delay: float = 0.0, *, adjacent_enabled: bool = False, radar_available: bool = False,
|
|
clock_pair_fn=None, honda_bosch_a_radar: bool = False, use_voacc: bool = False):
|
|
self.use_voacc = use_voacc
|
|
self.tracks: dict[int, Track] = {}
|
|
self.honda_bosch_a = None
|
|
if honda_bosch_a_radar:
|
|
from openpilot.starpilot.car.honda.radar import BoschALeadPolicy
|
|
self.honda_bosch_a = BoschALeadPolicy()
|
|
self.kalman_params = KalmanParams(1.0 / 15.0 if self.honda_bosch_a is not None else DT_MDL)
|
|
self.lead_prob_filters = [FirstOrderFilter(0.0, 0.2, DT_MDL) for _ in range(2)]
|
|
|
|
self.v_ego = 0.0
|
|
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
|
|
self.radar_state_valid = False
|
|
|
|
self.ready = False
|
|
self.adjacent_enabled = adjacent_enabled
|
|
self.adjacent_radar_available = radar_available
|
|
self.adjacent_clock_pair_fn = clock_pair_fn or _clock_pair
|
|
self.adjacent_session = secrets.token_hex(16)
|
|
self.adjacent_sequence = 0
|
|
self.adjacent_offset_ns: int | None = None
|
|
self.adjacent_barrier_mono_ns = 0
|
|
self.adjacent_barrier_boot_ns = 0
|
|
self.adjacent_last_mono_ns = 0
|
|
self.adjacent_last_boot_ns = 0
|
|
self.adjacent_observation: AdjacentObservation = UNKNOWN
|
|
self.adjacent_sources: tuple[int, int, int, int] = (0, 0, 0, 0)
|
|
self.adjacent_valid_until_ns = 0
|
|
|
|
def _qualified_adjacent(self, sm: messaging.SubMaster, rr: car.RadarData) -> None:
|
|
pair = self.adjacent_clock_pair_fn()
|
|
self.adjacent_observation = UNKNOWN
|
|
self.adjacent_sources = (0, 0, 0, 0)
|
|
self.adjacent_valid_until_ns = 0
|
|
if pair is None or len(pair) != 3:
|
|
return
|
|
now_mono, now_boot, skew = pair
|
|
if not all(type(value) is int for value in pair) or not 0 <= skew <= CLOCK_PAIR_MAX_SKEW_NS:
|
|
return
|
|
offset = now_boot - now_mono
|
|
regressed = now_mono <= self.adjacent_last_mono_ns or now_boot <= self.adjacent_last_boot_ns
|
|
previous_mono, previous_boot = self.adjacent_last_mono_ns, self.adjacent_last_boot_ns
|
|
self.adjacent_last_mono_ns = max(now_mono, previous_mono)
|
|
self.adjacent_last_boot_ns = max(now_boot, previous_boot)
|
|
if (self.adjacent_offset_ns is None or regressed or
|
|
abs(offset - self.adjacent_offset_ns) > max(CLOCK_PAIR_MAX_SKEW_NS, skew * 2)):
|
|
self.adjacent_offset_ns = offset
|
|
self.adjacent_barrier_mono_ns = max(now_mono, previous_mono)
|
|
self.adjacent_barrier_boot_ns = max(now_boot, previous_boot)
|
|
return
|
|
radar_stamp = int(sm.logMonoTime['radarTracks'])
|
|
model_stamp = int(sm.logMonoTime['modelV2'])
|
|
car_stamp = int(sm.logMonoTime['carState'])
|
|
camera_eof = int(sm['modelV2'].timestampEof)
|
|
self.adjacent_sources = (radar_stamp, model_stamp, car_stamp, camera_eof)
|
|
primary_ids = frozenset(
|
|
int(getattr(lead, 'radarTrackId', -1)) for lead in (self.radar_state.leadOne, self.radar_state.leadTwo)
|
|
if bool(getattr(lead, 'present', False)) and bool(getattr(lead, 'radar', False)) and int(getattr(lead, 'radarTrackId', -1)) >= 0
|
|
)
|
|
frame = AdjacentFrame(
|
|
model=sm['modelV2'],
|
|
tracks=tuple(AdjacentTrack(track.identifier, track.dRel, track.yRel, track.vLead) for track in self.tracks.values()),
|
|
primary_track_ids=primary_ids,
|
|
radar_available=self.adjacent_radar_available,
|
|
radar_valid=bool(sm.valid['radarTracks'] and sm.updated['radarTracks']),
|
|
radar_error_free=not any(rr.errors.to_dict().values()),
|
|
model_valid=bool(sm.valid['modelV2']),
|
|
car_valid=bool(sm.valid['carState']),
|
|
standstill=bool(sm['carState'].standstill),
|
|
ego_speed_mps=self.v_ego,
|
|
radar=MonoSource(radar_stamp, int(sm.recv_time['radarTracks'] * 1e9)),
|
|
model_source=MonoSource(model_stamp, int(sm.recv_time['modelV2'] * 1e9)),
|
|
car=MonoSource(car_stamp, int(sm.recv_time['carState'] * 1e9)),
|
|
model_eof_boot_ns=camera_eof,
|
|
now_mono_ns=now_mono,
|
|
now_boot_ns=now_boot,
|
|
expected_boot_minus_mono_ns=self.adjacent_offset_ns,
|
|
barrier_mono_ns=self.adjacent_barrier_mono_ns,
|
|
barrier_boot_ns=self.adjacent_barrier_boot_ns,
|
|
sample_skew_ns=skew,
|
|
)
|
|
self.adjacent_observation = evaluate(frame)
|
|
if self.adjacent_observation.ambiguous is not None:
|
|
self.adjacent_valid_until_ns = min(
|
|
frame.radar.producer_ns + SOURCE_MAX_AGE_NS,
|
|
frame.radar.receipt_ns + SOURCE_MAX_AGE_NS,
|
|
frame.model_source.producer_ns + MODEL_MAX_AGE_NS,
|
|
frame.model_source.receipt_ns + MODEL_MAX_AGE_NS,
|
|
frame.car.producer_ns + SOURCE_MAX_AGE_NS,
|
|
frame.car.receipt_ns + SOURCE_MAX_AGE_NS,
|
|
frame.model_eof_boot_ns - frame.expected_boot_minus_mono_ns - frame.sample_skew_ns + MODEL_EOF_MAX_AGE_NS,
|
|
)
|
|
|
|
def update(self, sm: messaging.SubMaster, rr: car.RadarData):
|
|
self.ready = sm.seen['modelV2']
|
|
|
|
if sm.recv_frame['carState'] != self.last_v_ego_frame:
|
|
self.v_ego = sm['carState'].vEgo
|
|
self.v_ego_hist.append(self.v_ego)
|
|
self.last_v_ego_frame = sm.recv_frame['carState']
|
|
|
|
if self.honda_bosch_a is not None:
|
|
self.honda_bosch_a.update_tracks(sm, rr, self)
|
|
else:
|
|
ar_pts = {pt.trackId: [pt.dRel, pt.yRel, pt.vRel] for pt in rr.points}
|
|
|
|
# *** 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]
|
|
|
|
# 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(ids, v_lead, self.kalman_params)
|
|
self.tracks[ids].update(rpt[0], rpt[1], rpt[2], v_lead)
|
|
|
|
# *** publish radarState ***
|
|
self.radar_state_valid = sm.all_checks()
|
|
self.radar_state = log.RadarState.new_message()
|
|
self.radar_state.mdMonoTime = sm.logMonoTime['modelV2']
|
|
self.radar_state.radarErrors = rr.errors
|
|
|
|
if len(sm['modelV2'].velocity.x):
|
|
model_v_ego = sm['modelV2'].velocity.x[0]
|
|
else:
|
|
model_v_ego = self.v_ego
|
|
leads_v3 = sm['modelV2'].leadsV3
|
|
if len(leads_v3) > 1:
|
|
for i in range(2):
|
|
# Asymmetric filter on lead prob to keep lead when uncertain
|
|
lead_prob = leads_v3[i].prob
|
|
if lead_prob > self.lead_prob_filters[i].x:
|
|
self.lead_prob_filters[i].x = lead_prob
|
|
else:
|
|
self.lead_prob_filters[i].update(lead_prob)
|
|
|
|
if self.use_voacc:
|
|
self.radar_state.leadOne = get_lead(self.v_ego, self.ready, {}, leads_v3[0], model_v_ego,
|
|
self.lead_prob_filters[0].x, low_speed_override=False)
|
|
self.radar_state.leadTwo = get_lead(self.v_ego, self.ready, {}, leads_v3[1], model_v_ego,
|
|
self.lead_prob_filters[1].x, low_speed_override=False)
|
|
elif self.honda_bosch_a is not None:
|
|
self.radar_state.leadOne, self.radar_state.leadTwo = self.honda_bosch_a.leads(sm, self, model_v_ego)
|
|
else:
|
|
self.radar_state.leadOne = get_lead(self.v_ego, self.ready, self.tracks, leads_v3[0], model_v_ego, self.lead_prob_filters[0].x, low_speed_override=True)
|
|
self.radar_state.leadTwo = get_lead(self.v_ego, self.ready, self.tracks, leads_v3[1], model_v_ego,
|
|
self.lead_prob_filters[1].x, low_speed_override=False)
|
|
if self.adjacent_enabled:
|
|
self._qualified_adjacent(sm, rr)
|
|
|
|
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
|
|
pm.send("radarState", radar_msg)
|
|
|
|
if not self.adjacent_enabled:
|
|
return
|
|
|
|
adjacent_msg = messaging.new_message('starpilotRadarState')
|
|
adjacent_msg.valid = self.adjacent_observation.ambiguous is not None
|
|
self.adjacent_sequence += 1
|
|
status = adjacent_msg.starpilotRadarState.qualifiedAdjacent
|
|
status.version = 1
|
|
status.status = ('unknown' if self.adjacent_observation.ambiguous is None else
|
|
'ambiguous' if self.adjacent_observation.ambiguous else 'clear')
|
|
status.producerSessionId = self.adjacent_session
|
|
status.sequence = self.adjacent_sequence
|
|
status.radarTracksMonoTime, status.modelMonoTime, status.carStateMonoTime, status.cameraEofBootTime = self.adjacent_sources
|
|
observed = self.adjacent_observation.observed_mono_ns
|
|
if observed is not None:
|
|
status.observedMonoTime = observed
|
|
status.validUntilMonoTime = self.adjacent_valid_until_ns
|
|
for name, candidate in (('left', self.adjacent_observation.left), ('right', self.adjacent_observation.right)):
|
|
if candidate is not None:
|
|
wire = getattr(status, name)
|
|
wire.present = True
|
|
wire.trackId = candidate.track_id
|
|
wire.distanceM = candidate.distance_m
|
|
wire.lateralM = candidate.lateral_m
|
|
wire.speedMps = candidate.speed_mps
|
|
pm.send('starpilotRadarState', adjacent_msg)
|
|
|
|
|
|
# fuses camera and radar data for best lead detection
|
|
def main() -> None:
|
|
config_realtime_process(5, Priority.CTRL_LOW)
|
|
|
|
# wait for stats about the car to come in from controls
|
|
cloudlog.info("radard is waiting for CarParams")
|
|
params = Params()
|
|
CP = messaging.log_from_bytes(params.get("CarParams", block=True), car.CarParams)
|
|
cloudlog.info("radard got CarParams")
|
|
|
|
# *** setup messaging
|
|
sm = messaging.SubMaster(['modelV2', 'carState', 'radarTracks'], poll='modelV2')
|
|
adjacent_enabled = feature_enabled(params, CP, 'conditional', os.environ) and 'REPLAY' not in os.environ
|
|
pm = messaging.PubMaster(['radarState', 'starpilotRadarState'] if adjacent_enabled else ['radarState'])
|
|
|
|
from opendbc.car.honda.bosch_a_radar import VERIFIED_BOSCH_A_CARS
|
|
honda_bosch_a = CP.brand == "honda" and CP.carFingerprint in VERIFIED_BOSCH_A_CARS and not CP.radarUnavailable
|
|
RD = RadarD(CP.radarDelay, adjacent_enabled=adjacent_enabled, radar_available=not CP.radarUnavailable,
|
|
honda_bosch_a_radar=honda_bosch_a, use_voacc=voacc_selected(params, CP))
|
|
set_core_affinity([3, 5])
|
|
|
|
while 1:
|
|
sm.update()
|
|
|
|
RD.update(sm, sm['radarTracks'])
|
|
RD.publish(pm)
|
|
|
|
|
|
if __name__ == "__main__":
|
|
main()
|