mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-06-28 01:52:06 +08:00
radar vs voacc
This commit is contained in:
firestar5683
@@ -142,8 +142,19 @@ def g90_low_speed_radar_lead_sane(track: Track, v_ego: float) -> bool:
|
||||
abs(track.yRel) < G90_RADAR_LOW_SPEED_MAX_Y)
|
||||
|
||||
|
||||
def track_matches_vision(track: Track, lead: capnp._DynamicStructReader, v_ego: float, *,
|
||||
dist_scale: float, dist_floor: float, vel_limit: float,
|
||||
y_std_scale: float, y_floor: float) -> bool:
|
||||
offset_vision_dist = lead.x[0] - RADAR_TO_CAMERA
|
||||
dist_sane = abs(track.dRel - offset_vision_dist) < max(abs(offset_vision_dist) * dist_scale, dist_floor)
|
||||
vel_sane = (abs(track.vRel + v_ego - lead.v[0]) < vel_limit) or (v_ego + track.vRel > 3)
|
||||
lat_sane = abs(track.yRel + lead.y[0]) < max(y_floor, y_std_scale * max(float(lead.yStd[0]), 0.2))
|
||||
return dist_sane and vel_sane and lat_sane
|
||||
|
||||
|
||||
def match_vision_to_track(v_ego: float, lead: capnp._DynamicStructReader, model_data: capnp._DynamicStructReader, tracks: dict[int, Track],
|
||||
starpilot_toggles: SimpleNamespace, g90_radar_filter: bool = False):
|
||||
starpilot_toggles: SimpleNamespace, g90_radar_filter: bool = False,
|
||||
preferred_track_id: int = -1):
|
||||
if model_data.meta.laneChangeState == LaneChangeState.laneChangeStarting and getattr(starpilot_toggles, "human_lane_changes", False):
|
||||
direction = model_data.meta.laneChangeDirection
|
||||
if direction == LaneChangeDirection.left:
|
||||
@@ -157,9 +168,8 @@ def match_vision_to_track(v_ego: float, lead: capnp._DynamicStructReader, model_
|
||||
if not tracks:
|
||||
return None
|
||||
|
||||
offset_vision_dist = lead.x[0] - RADAR_TO_CAMERA
|
||||
|
||||
def prob(c):
|
||||
offset_vision_dist = lead.x[0] - RADAR_TO_CAMERA
|
||||
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])
|
||||
@@ -169,10 +179,21 @@ def match_vision_to_track(v_ego: float, lead: capnp._DynamicStructReader, model_
|
||||
|
||||
# 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:
|
||||
if track_matches_vision(track, lead, v_ego,
|
||||
dist_scale=0.25, dist_floor=5.0,
|
||||
vel_limit=10.0, y_std_scale=1.0, y_floor=1.0):
|
||||
return track
|
||||
|
||||
# Some vehicles intermittently drop a good radar match on large leads (semis are
|
||||
# a common offender). If the same track is still present and only missed the
|
||||
# strict vision gate by a small margin, keep the previous radar match instead of
|
||||
# oscillating between radar and vision estimates.
|
||||
preferred_track = tracks.get(preferred_track_id)
|
||||
if preferred_track is not None and preferred_track.cnt >= 3:
|
||||
if track_matches_vision(preferred_track, lead, v_ego,
|
||||
dist_scale=0.40, dist_floor=8.0,
|
||||
vel_limit=13.0, y_std_scale=2.0, y_floor=1.5):
|
||||
return preferred_track
|
||||
return None
|
||||
|
||||
|
||||
@@ -199,13 +220,15 @@ 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, model_data: capnp._DynamicStructReader, standstill: bool,
|
||||
starpilot_plan: capnp._DynamicStructReader, starpilot_toggles: SimpleNamespace,
|
||||
low_speed_override: bool = True, g90_radar_filter: bool = False, lead_prob: float | None = None) -> dict[str, Any]:
|
||||
low_speed_override: bool = True, g90_radar_filter: bool = False, lead_prob: float | None = None,
|
||||
preferred_track_id: int = -1) -> dict[str, Any]:
|
||||
lead_detection_probability = float(getattr(starpilot_toggles, "lead_detection_probability", 0.35))
|
||||
filtered_lead_prob = float(lead_msg.prob if lead_prob is None else lead_prob)
|
||||
|
||||
# Determine leads, this is where the essential logic happens
|
||||
if len(tracks) > 0 and ready and filtered_lead_prob > lead_detection_probability:
|
||||
track = match_vision_to_track(v_ego, lead_msg, model_data, tracks, starpilot_toggles, g90_radar_filter)
|
||||
track = match_vision_to_track(v_ego, lead_msg, model_data, tracks, starpilot_toggles, g90_radar_filter,
|
||||
preferred_track_id=preferred_track_id)
|
||||
else:
|
||||
track = None
|
||||
|
||||
@@ -255,6 +278,7 @@ class RadarD:
|
||||
self.kalman_params = KalmanParams(radar_ts)
|
||||
self.g90_radar_filter = g90_radar_filter
|
||||
self.lead_prob_filters = [FirstOrderFilter(0.0, 0.2, radar_ts) for _ in range(2)]
|
||||
self.prev_lead_track_ids = [-1, -1]
|
||||
|
||||
self.v_ego = 0.0
|
||||
self.v_ego_hist = deque([0.0], maxlen=int(round(delay / DT_MDL)) + 1)
|
||||
@@ -319,10 +343,18 @@ class RadarD:
|
||||
|
||||
self.radar_state.leadOne = get_lead(self.v_ego, self.ready, self.tracks, leads_v3[0], model_v_ego, sm['modelV2'],
|
||||
sm['carState'].standstill, sm['starpilotPlan'], self.starpilot_toggles, low_speed_override=True,
|
||||
g90_radar_filter=self.g90_radar_filter, lead_prob=self.lead_prob_filters[0].x)
|
||||
g90_radar_filter=self.g90_radar_filter, lead_prob=self.lead_prob_filters[0].x,
|
||||
preferred_track_id=self.prev_lead_track_ids[0])
|
||||
self.radar_state.leadTwo = get_lead(self.v_ego, self.ready, self.tracks, leads_v3[1], model_v_ego, sm['modelV2'],
|
||||
sm['carState'].standstill, sm['starpilotPlan'], self.starpilot_toggles, low_speed_override=False,
|
||||
g90_radar_filter=self.g90_radar_filter, lead_prob=self.lead_prob_filters[1].x)
|
||||
g90_radar_filter=self.g90_radar_filter, lead_prob=self.lead_prob_filters[1].x,
|
||||
preferred_track_id=self.prev_lead_track_ids[1])
|
||||
|
||||
for i, lead in enumerate((self.radar_state.leadOne, self.radar_state.leadTwo)):
|
||||
if lead.status and getattr(lead, "radar", False):
|
||||
self.prev_lead_track_ids[i] = int(getattr(lead, "radarTrackId", -1))
|
||||
elif (not lead.status) or (self.prev_lead_track_ids[i] not in self.tracks):
|
||||
self.prev_lead_track_ids[i] = -1
|
||||
|
||||
if self.ready and (self.starpilot_toggles.adjacent_lead_tracking or self.starpilot_toggles.human_lane_changes):
|
||||
self.starpilot_radar_state.leadLeft = get_adjacent_lead(self.tracks, sm['carState'].standstill, sm['modelV2'], left=True)
|
||||
|
||||
@@ -1,13 +1,34 @@
|
||||
from types import SimpleNamespace
|
||||
|
||||
import cereal.messaging as messaging
|
||||
from cereal import log
|
||||
|
||||
from opendbc.car.toyota.values import CAR as TOYOTA
|
||||
from openpilot.selfdrive.test.process_replay import replay_process_with_name
|
||||
from openpilot.selfdrive.controls.radard import g90_low_speed_radar_lead_sane, g90_radar_lead_lateral_sane
|
||||
from openpilot.selfdrive.controls.radard import (
|
||||
g90_low_speed_radar_lead_sane,
|
||||
g90_radar_lead_lateral_sane,
|
||||
match_vision_to_track,
|
||||
)
|
||||
|
||||
|
||||
class TestLeads:
|
||||
@staticmethod
|
||||
def make_track(d_rel: float, y_rel: float, v_rel: float, cnt: int = 5):
|
||||
return SimpleNamespace(dRel=d_rel, yRel=y_rel, vRel=v_rel, cnt=cnt)
|
||||
|
||||
@staticmethod
|
||||
def make_lead(x: float, y: float, v: float, x_std: float = 1.5, y_std: float = 0.3, v_std: float = 1.0):
|
||||
return SimpleNamespace(x=[x], y=[y], v=[v], xStd=[x_std], yStd=[y_std], vStd=[v_std])
|
||||
|
||||
@staticmethod
|
||||
def make_model_data():
|
||||
model = log.ModelDataV2.new_message()
|
||||
model.init('laneLines', 4)
|
||||
model.meta.laneChangeState = 0
|
||||
model.meta.laneChangeDirection = 0
|
||||
return model
|
||||
|
||||
def test_g90_radar_filters_side_tracks(self):
|
||||
side_track = SimpleNamespace(dRel=13.0, yRel=-10.38, cnt=10)
|
||||
centered_track = SimpleNamespace(dRel=10.8, yRel=-0.21, cnt=5)
|
||||
@@ -22,6 +43,40 @@ class TestLeads:
|
||||
assert g90_low_speed_radar_lead_sane(centered_track, 2.0)
|
||||
assert not g90_low_speed_radar_lead_sane(far_low_speed_track, 3.5)
|
||||
|
||||
def test_match_vision_to_track_sticky_holds_preferred_track(self):
|
||||
v_ego = 21.2
|
||||
lead = self.make_lead(x=37.72, y=0.07, v=20.03)
|
||||
preferred_track = self.make_track(d_rel=46.95, y_rel=0.05, v_rel=-0.58, cnt=8)
|
||||
tracks = {3647: preferred_track}
|
||||
|
||||
track = match_vision_to_track(
|
||||
v_ego,
|
||||
lead,
|
||||
self.make_model_data(),
|
||||
tracks,
|
||||
SimpleNamespace(human_lane_changes=False),
|
||||
preferred_track_id=3647,
|
||||
)
|
||||
|
||||
assert track is preferred_track
|
||||
|
||||
def test_match_vision_to_track_sticky_rejects_bad_preferred_track(self):
|
||||
v_ego = 21.2
|
||||
lead = self.make_lead(x=37.72, y=0.07, v=20.03)
|
||||
preferred_track = self.make_track(d_rel=58.0, y_rel=2.8, v_rel=-4.5, cnt=8)
|
||||
tracks = {3647: preferred_track}
|
||||
|
||||
track = match_vision_to_track(
|
||||
v_ego,
|
||||
lead,
|
||||
self.make_model_data(),
|
||||
tracks,
|
||||
SimpleNamespace(human_lane_changes=False),
|
||||
preferred_track_id=3647,
|
||||
)
|
||||
|
||||
assert track is None
|
||||
|
||||
def test_radar_fault(self):
|
||||
# if there's no radar-related can traffic, radard should either not respond or respond with an error
|
||||
# this is tightly coupled with underlying car radar_interface implementation, but it's a good sanity check
|
||||
|
||||
Reference in New Issue
Block a user