mawei/StarPilot
1
0
Fork 0
mirror of https://github.com/firestar5683/StarPilot.git synced 2026-07-03 04:22:09 +08:00
Code Issues Packages Projects Releases Wiki Activity

planner work

Browse Source
This commit is contained in:
firestar5683
2026-06-14 16:38:10 -05:00
parent 7bd73401cd
commit 2e75486d16
2 changed files with 19 additions and 134 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/controls/radard.py
+18 -53
View File
@@ -176,7 +176,7 @@ def match_vision_to_track(v_ego: float, lead: capnp._DynamicStructReader, model_
return None
def get_RadarState_from_vision(lead_msg: capnp._DynamicStructReader, v_ego: float, model_v_ego: float):
def get_RadarState_from_vision(lead_msg: capnp._DynamicStructReader, v_ego: float, model_v_ego: float, model_prob: float):
prev_aLeadK = getattr(get_RadarState_from_vision, "prev_aLeadK", 0.0)
blended_aLeadK = 0.8 * float(lead_msg.a[0]) + 0.2 * prev_aLeadK
get_RadarState_from_vision.prev_aLeadK = blended_aLeadK
@@ -189,69 +189,31 @@ def get_RadarState_from_vision(lead_msg: capnp._DynamicStructReader, v_ego: floa
"aLeadK": blended_aLeadK,
"aLeadTau": 0.3,
"fcw": False,
"modelProb": float(lead_msg.prob),
"modelProb": float(model_prob),
"status": True,
"radar": False,
"radarTrackId": -1,
}
VISION_DUPLICATE_LEAD_MAX_DREL_DIFF = 0.75
VISION_DUPLICATE_LEAD_MAX_VLEAD_DIFF = 0.5
VISION_DUPLICATE_LEAD_MAX_YREL_DIFF = 0.4
VISION_DUPLICATE_LEAD_MIN_MODEL_PROB = 0.7
def get_lead_field(lead: Any, field: str, default: Any) -> Any:
if isinstance(lead, dict):
return lead.get(field, default)
return getattr(lead, field, default)
def leads_are_duplicate(lead_one: Any, lead_two: Any) -> bool:
if not get_lead_field(lead_one, "status", False) or not get_lead_field(lead_two, "status", False):
return False
lead_one_radar = bool(get_lead_field(lead_one, "radar", False))
lead_two_radar = bool(get_lead_field(lead_two, "radar", False))
if lead_one_radar and lead_two_radar:
lead_one_track_id = int(get_lead_field(lead_one, "radarTrackId", -1))
lead_two_track_id = int(get_lead_field(lead_two, "radarTrackId", -1))
return lead_one_track_id != -1 and lead_one_track_id == lead_two_track_id
if lead_one_radar or lead_two_radar:
return False
lead_one_prob = float(get_lead_field(lead_one, "modelProb", 0.0))
lead_two_prob = float(get_lead_field(lead_two, "modelProb", 0.0))
if min(lead_one_prob, lead_two_prob) < VISION_DUPLICATE_LEAD_MIN_MODEL_PROB:
return False
return (
abs(float(get_lead_field(lead_one, "dRel", 0.0)) - float(get_lead_field(lead_two, "dRel", 0.0))) <= VISION_DUPLICATE_LEAD_MAX_DREL_DIFF and
abs(float(get_lead_field(lead_one, "vLead", 0.0)) - float(get_lead_field(lead_two, "vLead", 0.0))) <= VISION_DUPLICATE_LEAD_MAX_VLEAD_DIFF and
abs(float(get_lead_field(lead_one, "yRel", 0.0)) - float(get_lead_field(lead_two, "yRel", 0.0))) <= VISION_DUPLICATE_LEAD_MAX_YREL_DIFF
)
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) -> dict[str, Any]:
low_speed_override: bool = True, g90_radar_filter: bool = False, lead_prob: float | None = None) -> 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 lead_msg.prob > lead_detection_probability:
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)
else:
track = None
lead_dict = {'status': False}
if track is not None:
lead_dict = track.get_RadarState(lead_msg.prob)
elif (track is None) and ready and (lead_msg.prob > lead_detection_probability):
lead_dict = get_RadarState_from_vision(lead_msg, v_ego, model_v_ego)
lead_dict = track.get_RadarState(filtered_lead_prob)
elif (track is None) and ready and (filtered_lead_prob > lead_detection_probability):
lead_dict = get_RadarState_from_vision(lead_msg, v_ego, model_v_ego, filtered_lead_prob)
if low_speed_override:
if g90_radar_filter:
@@ -292,6 +254,7 @@ class RadarD:
self.tracks: dict[int, Track] = {}
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.v_ego = 0.0
self.v_ego_hist = deque([0.0], maxlen=int(round(delay / DT_MDL)) + 1)
@@ -347,17 +310,19 @@ class RadarD:
leads_v3 = sm['modelV2'].leadsV3
if len(leads_v3) > 1:
for i in range(2):
lead_prob = float(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)
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)
g90_radar_filter=self.g90_radar_filter, lead_prob=self.lead_prob_filters[0].x)
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)
# The model exposes two lead slots, but both can occasionally fuse to the
# same radar object. Publishing that as two separate leads makes MPC churn
# between lead0/lead1 even though the scene only has one physical target.
if leads_are_duplicate(self.radar_state.leadOne, self.radar_state.leadTwo):
self.radar_state.leadTwo = {'status': False}
g90_radar_filter=self.g90_radar_filter, lead_prob=self.lead_prob_filters[1].x)
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)
selfdrive/controls/tests/test_leads.py
+1 -81
View File
@@ -4,7 +4,7 @@ import cereal.messaging as messaging
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, leads_are_duplicate
from openpilot.selfdrive.controls.radard import g90_low_speed_radar_lead_sane, g90_radar_lead_lateral_sane
class TestLeads:
@@ -22,86 +22,6 @@ 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_duplicate_radar_leads_share_track(self):
lead_one = {
"status": True,
"radar": True,
"radarTrackId": 20293,
}
lead_two = {
"status": True,
"radar": True,
"radarTrackId": 20293,
}
different_track = {
"status": True,
"radar": True,
"radarTrackId": 20326,
}
vision_only = {
"status": True,
"radar": False,
"radarTrackId": -1,
}
assert leads_are_duplicate(lead_one, lead_two)
assert not leads_are_duplicate(lead_one, different_track)
assert not leads_are_duplicate(lead_one, vision_only)
def test_duplicate_vision_leads_are_deduped(self):
lead_one = {
"status": True,
"radar": False,
"radarTrackId": -1,
"dRel": 48.3,
"vLead": 18.9,
"yRel": 0.14,
"modelProb": 0.99,
}
lead_two = {
"status": True,
"radar": False,
"radarTrackId": -1,
"dRel": 48.4,
"vLead": 19.0,
"yRel": 0.14,
"modelProb": 1.00,
}
distinct_lead = {
"status": True,
"radar": False,
"radarTrackId": -1,
"dRel": 48.3,
"vLead": 18.9,
"yRel": 1.10,
"modelProb": 0.99,
}
assert leads_are_duplicate(lead_one, lead_two)
assert not leads_are_duplicate(lead_one, distinct_lead)
def test_duplicate_lead_helper_supports_attribute_objects(self):
lead_one = SimpleNamespace(
status=True,
radar=False,
radarTrackId=-1,
dRel=32.1,
vLead=14.2,
yRel=0.03,
modelProb=0.98,
)
lead_two = SimpleNamespace(
status=True,
radar=False,
radarTrackId=-1,
dRel=32.2,
vLead=14.1,
yRel=0.05,
modelProb=0.97,
)
assert leads_are_duplicate(lead_one, lead_two)
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