brianhansonxyz/clearpilot
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

wip

Browse Source
This commit is contained in:
Your Name
2024-04-27 13:44:34 -05:00
parent ea1aad5ed1
commit 2fbe9dbea1
25 changed files with 1501 additions and 532 deletions
Download Patch File Download Diff File Expand all files Collapse all files

14
selfdrive/controls/radard.py
View File

@@ -2,7 +2,7 @@
import importlib
import math
from collections import deque
from typing import Optional, Dict, Any
from typing import Any, Optional
import capnp
from cereal import messaging, log, car
@@ -125,7 +125,7 @@ def laplacian_pdf(x: float, mu: float, b: float):
return math.exp(-abs(x-mu)/b)
def match_vision_to_track(v_ego: float, lead: capnp._DynamicStructReader, tracks: Dict[int, Track]):
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):
@@ -133,7 +133,7 @@ def match_vision_to_track(v_ego: float, lead: capnp._DynamicStructReader, tracks
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 is isn't exactly right, but good heuristic
# This isn't exactly right, but it's a good heuristic
return prob_d * prob_y * prob_v
track = max(tracks.values(), key=prob)
@@ -166,8 +166,8 @@ def get_RadarState_from_vision(lead_msg: capnp._DynamicStructReader, v_ego: floa
}
def get_lead(v_ego: float, ready: bool, tracks: Dict[int, Track], lead_msg: capnp._DynamicStructReader,
model_v_ego: float, lead_detection_threshold: float = .5, low_speed_override: bool = True) -> Dict[str, Any]:
def get_lead(v_ego: float, ready: bool, tracks: dict[int, Track], lead_msg: capnp._DynamicStructReader,
model_v_ego: float, lead_detection_threshold: 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 > lead_detection_threshold:
track = match_vision_to_track(v_ego, lead_msg, tracks)
@@ -196,14 +196,14 @@ class RadarD:
def __init__(self, radar_ts: float, delay: int = 0):
self.current_time = 0.0
self.tracks: Dict[int, Track] = {}
self.tracks: dict[int, Track] = {}
self.kalman_params = KalmanParams(radar_ts)
self.v_ego = 0.0
self.v_ego_hist = deque([0.0], maxlen=delay+1)
self.last_v_ego_frame = -1
self.radar_state: Optional[capnp._DynamicStructBuilder] = None
self.radar_state: capnp._DynamicStructBuilder | None = None
self.radar_state_valid = False
self.ready = False