mawei/StarPilot
1
0
Fork 0
mirror of https://github.com/firestar5683/StarPilot.git synced 2026-08-21 16:23:46 +08:00
Code Issues Packages Projects Releases Wiki Activity

I said he a radar boi, I said see you l8er boi

Browse Source
This commit is contained in:
firestar5683
2026-04-24 13:57:16 -05:00
parent aa089fc64f
commit 9d2a80504b
6 changed files with 135 additions and 28 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/controls/tests/test_longitudinal_planner.py
+23 -2
View File
@@ -26,7 +26,21 @@ def make_lead(*, status: bool, d_rel: float = 200.0, v_lead: float = 0.0, a_lead
return lead
def make_model(v_ego: float, desired_accel: float, gas_press_prob: float = 1.0):
def fill_model_lead(lead_msg, lead, prob: float):
lead_msg.prob = float(prob)
lead_msg.probTime = 0.0
lead_msg.t = [float(t) for t in ModelConstants.LEAD_T_IDXS]
lead_msg.x = [float(lead.dRel + lead.vLead * t) for t in ModelConstants.LEAD_T_IDXS]
lead_msg.xStd = [0.5] * len(ModelConstants.LEAD_T_IDXS)
lead_msg.y = [0.0] * len(ModelConstants.LEAD_T_IDXS)
lead_msg.yStd = [0.5] * len(ModelConstants.LEAD_T_IDXS)
lead_msg.v = [float(lead.vLead)] * len(ModelConstants.LEAD_T_IDXS)
lead_msg.vStd = [0.5] * len(ModelConstants.LEAD_T_IDXS)
lead_msg.a = [float(lead.aLeadK)] * len(ModelConstants.LEAD_T_IDXS)
lead_msg.aStd = [0.5] * len(ModelConstants.LEAD_T_IDXS)
def make_model(v_ego: float, desired_accel: float, gas_press_prob: float = 1.0, lead_one=None, lead_two=None):
model = log.ModelDataV2.new_message()
t_idxs = ModelConstants.T_IDXS
@@ -45,6 +59,13 @@ def make_model(v_ego: float, desired_accel: float, gas_press_prob: float = 1.0):
model.acceleration.z = [0.0] * len(t_idxs)
model.acceleration.t = [float(t) for t in t_idxs]
model.init('leadsV3', 3)
fill_model_lead(model.leadsV3[0], lead_one if lead_one is not None else make_lead(status=False),
1.0 if lead_one is not None and lead_one.status else 0.0)
fill_model_lead(model.leadsV3[1], lead_two if lead_two is not None else make_lead(status=False),
1.0 if lead_two is not None and lead_two.status else 0.0)
fill_model_lead(model.leadsV3[2], make_lead(status=False), 0.0)
model.meta.disengagePredictions.gasPressProbs = [float(gas_press_prob)] * 6
model.action.desiredAcceleration = desired_accel
model.action.shouldStop = False
@@ -69,7 +90,7 @@ def make_sm(v_ego: float, desired_accel: float, min_accel: float, *, experimenta
forceDecel=False,
),
"liveParameters": SimpleNamespace(angleOffsetDeg=0.0),
"modelV2": make_model(v_ego, desired_accel, gas_press_prob=gas_press_prob),
"modelV2": make_model(v_ego, desired_accel, gas_press_prob=gas_press_prob, lead_one=lead_one, lead_two=lead_two),
"radarState": SimpleNamespace(
leadOne=lead_one if lead_one is not None else make_lead(status=False),
leadTwo=lead_two if lead_two is not None else make_lead(status=False),