diff --git a/cereal/log.capnp b/cereal/log.capnp index e491c8b2e..9db3e1d8c 100644 --- a/cereal/log.capnp +++ b/cereal/log.capnp @@ -1262,12 +1262,6 @@ struct LongitudinalPlan @0xe00b5b3eba12876c { solverExecutionTime @35 :Float32; - # lead trajectories the MPC solved against (13 points at T_IDXS) - leadTrajectoryX0 @40 :List(Float32); - leadTrajectoryV0 @41 :List(Float32); - leadTrajectoryX1 @42 :List(Float32); - leadTrajectoryV1 @43 :List(Float32); - enum LongitudinalPlanSource { cruise @0; lead0 @1; diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index 539431832..39f08b886 100755 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -15,7 +15,7 @@ from openpilot.common.realtime import DT_MDL from openpilot.common.swaglog import cloudlog from openpilot.selfdrive.controls.lib.lead_behavior import get_tracked_lead_catchup_bias # WARNING: imports outside of constants will not trigger a rebuild -from openpilot.selfdrive.modeld.constants import index_function, ModelConstants +from openpilot.selfdrive.modeld.constants import index_function if __name__ == '__main__': # generating code from openpilot.third_party.acados.acados_template import AcadosModel, AcadosOcp, AcadosOcpSolver @@ -30,7 +30,6 @@ EXPORT_DIR = os.path.join(LONG_MPC_DIR, "c_generated_code") JSON_FILE = os.path.join(LONG_MPC_DIR, "acados_ocp_long.json") SOURCES = ['lead0', 'lead1', 'cruise', 'e2e'] -LEAD_T_IDXS_MODEL = np.array(ModelConstants.LEAD_T_IDXS) X_DIM = 3 U_DIM = 1 @@ -350,8 +349,6 @@ class LongitudinalMpc: self.time_linearization = 0.0 self.time_integrator = 0.0 self.x0 = np.zeros(X_DIM) - self.lead_xv_0 = np.zeros((N+1, 2)) - self.lead_xv_1 = np.zeros((N+1, 2)) self.set_weights() def set_cost_weights(self, cost_weights, constraint_cost_weights): @@ -487,30 +484,13 @@ class LongitudinalMpc: lead_xv = np.column_stack((x_lead_traj, v_lead_traj)) return lead_xv - def process_lead(self, model_lead, radar_lead, tracking_lead=True): + def process_lead(self, lead, tracking_lead=True): v_ego = self.x0[1] - model_prob = float(getattr(model_lead, "prob", 0.0)) - model_x = getattr(model_lead, "x", ()) - model_v = getattr(model_lead, "v", ()) - - if tracking_lead and radar_lead is not None and radar_lead.status: - if bool(getattr(radar_lead, "radar", False)) and model_prob > 0.5 and len(model_x) and len(model_v): - x_lead_traj = float(radar_lead.dRel) + (np.asarray(model_x, dtype=np.float64) - float(model_x[0])) - v_lead_traj = float(radar_lead.vLead) + (np.asarray(model_v, dtype=np.float64) - float(model_v[0])) - - v_lead_0 = float(v_lead_traj[0]) - min_x_lead = ((v_ego + v_lead_0) / 2) * (v_ego - v_lead_0) / (-ACCEL_MIN * 2) - x_lead_traj[0] = max(x_lead_traj[0], min_x_lead) - v_lead_traj = np.clip(v_lead_traj, 0.0, 1e8) - - x_lead_mpc = np.maximum.accumulate(np.interp(T_IDXS, LEAD_T_IDXS_MODEL, x_lead_traj)) - v_lead_mpc = np.interp(T_IDXS, LEAD_T_IDXS_MODEL, v_lead_traj) - return np.column_stack((x_lead_mpc, v_lead_mpc)) - - x_lead = radar_lead.dRel - v_lead = radar_lead.vLead - a_lead = radar_lead.aLeadK - a_lead_tau = radar_lead.aLeadTau + if lead is not None and lead.status and tracking_lead: + x_lead = lead.dRel + v_lead = lead.vLead + a_lead = lead.aLeadK + a_lead_tau = lead.aLeadTau else: # Fake a fast lead car, so mpc can keep running in the same mode x_lead = 50.0 @@ -538,18 +518,15 @@ class LongitudinalMpc: self.cruise_min_a = min_a self.max_a = max_a - def update(self, radarstate, modelV2, v_cruise, x, v, a, j, danger_factor, t_follow, + def update(self, radarstate, v_cruise, x, v, a, j, danger_factor, t_follow, personality=log.LongitudinalPersonality.standard, tracking_lead=True): v_ego = self.x0[1] lead_one = radarstate.leadOne lead_two = radarstate.leadTwo self.status = (lead_one.status and tracking_lead) or lead_two.status - model_leads = modelV2.leadsV3 - lead_xv_0 = self.process_lead(model_leads[0] if len(model_leads) > 0 else None, lead_one, tracking_lead) - lead_xv_1 = self.process_lead(model_leads[1] if len(model_leads) > 1 else None, lead_two, tracking_lead) - self.lead_xv_0 = lead_xv_0 - self.lead_xv_1 = lead_xv_1 + lead_xv_0 = self.process_lead(lead_one, tracking_lead) + lead_xv_1 = self.process_lead(lead_two, tracking_lead) # To estimate a safe distance from a moving lead, we calculate how much stopping # distance that lead needs as a minimum. We can add that to the current distance @@ -613,9 +590,8 @@ class LongitudinalMpc: self.params[:,4] = t_follow self.run() - lead_one_prob = float(model_leads[0].prob) if len(model_leads) > 0 else float(lead_one.modelProb) if (np.any(lead_xv_0[FCW_IDXS,0] - self.x_sol[FCW_IDXS,0] < CRASH_DISTANCE) and - lead_one_prob > 0.9): + lead_one.modelProb > 0.9): self.crash_cnt += 1 else: self.crash_cnt = 0 diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index ff660a561..68777dbfe 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -454,7 +454,7 @@ class LongitudinalPlanner: dec_mpc_mode = self.get_mpc_mode() if not self.mlsim: self.mpc.mode = dec_mpc_mode - self.mpc.update(sm['radarState'], sm['modelV2'], v_cruise, x, v, a, j, + self.mpc.update(sm['radarState'], v_cruise, x, v, a, j, sm['starpilotPlan'].dangerFactor, sm['starpilotPlan'].tFollow, personality=personality, tracking_lead=lead_control_active) @@ -569,16 +569,10 @@ class LongitudinalPlanner: longitudinalPlan.accels = self.a_desired_trajectory.tolist() longitudinalPlan.jerks = self.j_desired_trajectory.tolist() - model_has_lead = len(sm['modelV2'].leadsV3) > 0 and sm['modelV2'].leadsV3[0].prob > 0.5 - longitudinalPlan.hasLead = bool(sm['radarState'].leadOne.status or model_has_lead) + longitudinalPlan.hasLead = sm['radarState'].leadOne.status longitudinalPlan.longitudinalPlanSource = self.mpc.source longitudinalPlan.fcw = self.fcw - longitudinalPlan.leadTrajectoryX0 = self.mpc.lead_xv_0[:, 0].tolist() - longitudinalPlan.leadTrajectoryV0 = self.mpc.lead_xv_0[:, 1].tolist() - longitudinalPlan.leadTrajectoryX1 = self.mpc.lead_xv_1[:, 0].tolist() - longitudinalPlan.leadTrajectoryV1 = self.mpc.lead_xv_1[:, 1].tolist() - longitudinalPlan.aTarget = float(self.output_a_target) longitudinalPlan.shouldStop = bool(self.output_should_stop) or sm['starpilotPlan'].forcingStopLength < 1 longitudinalPlan.allowBrake = True diff --git a/selfdrive/controls/radard.py b/selfdrive/controls/radard.py index 5cbcd2dc4..baf0fc7cc 100644 --- a/selfdrive/controls/radard.py +++ b/selfdrive/controls/radard.py @@ -19,13 +19,6 @@ from openpilot.starpilot.common.starpilot_variables import get_starpilot_toggles # Default lead acceleration decay set to 50% at 1s _LEAD_ACCEL_TAU = 0.6 -# Compare model v against dRel motion over a short window to correct sustained over-prediction. -_BIAS_FD_WINDOW_S = 2.0 -_BIAS_EMA_TAU = 0.5 -_BIAS_MAX_ABS = 8.0 -_BIAS_MIN_PROB = 0.5 -_BIAS_RESET_JUMP = 10.0 - # radar tracks SPEED, ACCEL = 0, 1 # Kalman filter states enum @@ -164,19 +157,16 @@ 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, - lead_bias: float = 0.0): +def get_RadarState_from_vision(lead_msg: capnp._DynamicStructReader, v_ego: float, model_v_ego: 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 - v_lead = max(0.0, v_ego + lead_msg.v[0] - model_v_ego - lead_bias) - lead_v_rel_pred = v_lead - 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_lead), - "vLeadK": float(v_lead), + "vRel": float(lead_msg.v[0] - model_v_ego), + "vLead": float(v_ego + (lead_msg.v[0] - model_v_ego)), + "vLeadK": float(v_ego + (lead_msg.v[0] - model_v_ego)), "aLeadK": blended_aLeadK, "aLeadTau": 0.3, "fcw": False, @@ -188,13 +178,13 @@ 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_prob: float, model_data: capnp._DynamicStructReader, standstill: bool, - starpilot_plan: capnp._DynamicStructReader, starpilot_toggles: SimpleNamespace, lead_bias: float = 0.0, + model_v_ego: float, model_data: capnp._DynamicStructReader, standstill: bool, + starpilot_plan: capnp._DynamicStructReader, starpilot_toggles: SimpleNamespace, low_speed_override: bool = True) -> dict[str, Any]: lead_detection_probability = float(getattr(starpilot_toggles, "lead_detection_probability", 0.35)) # Determine leads, this is where the essential logic happens - if len(tracks) > 0 and ready and lead_prob > lead_detection_probability: + if len(tracks) > 0 and ready and lead_msg.prob > lead_detection_probability: track = match_vision_to_track(v_ego, lead_msg, model_data, tracks, starpilot_toggles) else: track = None @@ -202,8 +192,8 @@ def get_lead(v_ego: float, ready: bool, tracks: dict[int, Track], lead_msg: capn 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_prob > lead_detection_probability): - lead_dict = get_RadarState_from_vision(lead_msg, v_ego, model_v_ego, lead_bias) + 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) if low_speed_override: low_speed_tracks = [c for c in tracks.values() if c.potential_low_speed_lead(v_ego)] @@ -240,12 +230,6 @@ class RadarD: self.tracks: dict[int, Track] = {} self.kalman_params = KalmanParams(radar_ts) - self.lead_prob_filters = [FirstOrderFilter(0.0, 0.2, DT_MDL) for _ in range(2)] - - self._bias_fd_k = int(round(_BIAS_FD_WINDOW_S / DT_MDL)) - self.lead_drel_hists = [deque(maxlen=self._bias_fd_k + 1) for _ in range(2)] - self.vego_hists_bias = [deque(maxlen=self._bias_fd_k + 1) for _ in range(2)] - self.bias_ema_filters = [FirstOrderFilter(0.0, _BIAS_EMA_TAU, 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) @@ -259,35 +243,6 @@ class RadarD: self.starpilot_radar_state = custom.StarPilotRadarState.new_message() self.starpilot_toggles = get_starpilot_toggles() - def _reset_lead_bias(self, idx: int) -> None: - self.lead_drel_hists[idx].clear() - self.vego_hists_bias[idx].clear() - self.bias_ema_filters[idx].x = 0.0 - - def _update_lead_bias(self, idx: int, lead_msg: capnp._DynamicStructReader) -> float: - # A newly-acquired or hopped lead does not have enough history for finite-difference biasing. - if len(lead_msg.x) == 0 or len(lead_msg.v) == 0 or float(lead_msg.prob) <= _BIAS_MIN_PROB: - self._reset_lead_bias(idx) - return 0.0 - - lead_x = float(lead_msg.x[0]) - if len(self.lead_drel_hists[idx]) and abs(lead_x - self.lead_drel_hists[idx][-1]) > _BIAS_RESET_JUMP: - self._reset_lead_bias(idx) - - self.lead_drel_hists[idx].append(lead_x) - self.vego_hists_bias[idx].append(self.v_ego) - - if len(self.lead_drel_hists[idx]) == self._bias_fd_k + 1 and len(self.vego_hists_bias[idx]) == self._bias_fd_k + 1: - dt_win = _BIAS_FD_WINDOW_S - v_ego_hist = self.vego_hists_bias[idx] - v_ego_avg = sum(v_ego_hist) / len(v_ego_hist) - a_ego_win = (v_ego_hist[-1] - v_ego_hist[0]) / dt_win - fd_vlead = v_ego_avg + (self.lead_drel_hists[idx][-1] - self.lead_drel_hists[idx][0]) / dt_win + a_ego_win * dt_win / 2 - bias_raw = np.clip(float(lead_msg.v[0]) - fd_vlead, -_BIAS_MAX_ABS, _BIAS_MAX_ABS) - self.bias_ema_filters[idx].update(bias_raw) - - return float(np.clip(self.bias_ema_filters[idx].x, -_BIAS_MAX_ABS, _BIAS_MAX_ABS)) - def update(self, sm: messaging.SubMaster, rr: car.RadarData): self.ready = sm.seen['modelV2'] self.current_time = 1e-9 * max(sm.logMonoTime.values()) @@ -330,18 +285,10 @@ class RadarD: leads_v3 = sm['modelV2'].leadsV3 if len(leads_v3) > 1: - lead_probs = [self.lead_prob_filters[i].update(leads_v3[i].prob) for i in range(2)] - lead_biases = [self._update_lead_bias(i, leads_v3[i]) for i in range(2)] - - self.radar_state.leadOne = get_lead(self.v_ego, self.ready, self.tracks, leads_v3[0], model_v_ego, lead_probs[0], - sm['modelV2'], sm['carState'].standstill, sm['starpilotPlan'], - self.starpilot_toggles, lead_bias=lead_biases[0], low_speed_override=True) - self.radar_state.leadTwo = get_lead(self.v_ego, self.ready, self.tracks, leads_v3[1], model_v_ego, lead_probs[1], - sm['modelV2'], sm['carState'].standstill, sm['starpilotPlan'], - self.starpilot_toggles, lead_bias=lead_biases[1], low_speed_override=False) - else: - for i in range(2): - self._reset_lead_bias(i) + 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) + 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) 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) diff --git a/selfdrive/controls/tests/test_longitudinal_planner.py b/selfdrive/controls/tests/test_longitudinal_planner.py index e945c3ed8..262a568ab 100644 --- a/selfdrive/controls/tests/test_longitudinal_planner.py +++ b/selfdrive/controls/tests/test_longitudinal_planner.py @@ -8,52 +8,25 @@ import pytest from cereal import log from opendbc.car.honda.interface import CarInterface from opendbc.car.honda.values import CAR -import openpilot.selfdrive.controls.radard as radard_mod from openpilot.selfdrive.controls.lib.longcontrol import LongCtrlState -from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import LongitudinalMpc from openpilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlanner, get_vehicle_min_accel -from openpilot.selfdrive.controls.radard import RadarD from openpilot.selfdrive.modeld.constants import ModelConstants, Plan -def make_lead(*, status: bool, d_rel: float = 200.0, v_lead: float = 0.0, a_lead: float = 0.0, - radar: bool = True, model_prob: float = 0.0, a_lead_tau: float = 0.3): +def make_lead(*, status: bool, d_rel: float = 200.0, v_lead: float = 0.0, a_lead: float = 0.0): lead = log.RadarState.LeadData.new_message() lead.status = status lead.dRel = d_rel lead.vLead = v_lead lead.vLeadK = v_lead lead.aLeadK = a_lead - lead.aLeadTau = a_lead_tau lead.vRel = 0.0 lead.aRel = 0.0 - lead.modelProb = model_prob - lead.radar = radar + lead.modelProb = 0.0 return lead -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_lead_msg(*, d_rel: float, v_lead: float, prob: float): - model = log.ModelDataV2.new_message() - model.init('leadsV3', 1) - fill_model_lead(model.leadsV3[0], make_lead(status=True, d_rel=d_rel, v_lead=v_lead), prob) - return model.leadsV3[0] - - -def make_model(v_ego: float, desired_accel: float, gas_press_prob: float = 1.0, lead_one=None, lead_two=None): +def make_model(v_ego: float, desired_accel: float, gas_press_prob: float = 1.0): model = log.ModelDataV2.new_message() t_idxs = ModelConstants.T_IDXS @@ -72,13 +45,6 @@ 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 @@ -103,7 +69,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, lead_one=lead_one, lead_two=lead_two), + "modelV2": make_model(v_ego, desired_accel, gas_press_prob=gas_press_prob), "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), @@ -259,38 +225,3 @@ def test_allow_throttle_hysteresis_filters_gas_prob_chatter(): planner.update(sm, toggles) assert planner.model_allow_throttle assert planner.allow_throttle - - -def test_radard_bias_resets_across_low_prob_lead_acquisition(monkeypatch): - monkeypatch.setattr(radard_mod, "get_starpilot_toggles", lambda sm=None: SimpleNamespace()) - rd = RadarD() - rd.v_ego = 33.0 - - for _ in range(rd._bias_fd_k + 5): - bias = rd._update_lead_bias(0, make_model_lead_msg(d_rel=120.0, v_lead=33.0, prob=0.01)) - assert bias == 0.0 - - bias = rd._update_lead_bias(0, make_model_lead_msg(d_rel=56.0, v_lead=33.0, prob=0.99)) - assert bias == 0.0 - assert len(rd.lead_drel_hists[0]) == 1 - assert len(rd.vego_hists_bias[0]) == 1 - - -def test_model_trajectory_requires_radar_anchor(): - radar_lead = make_lead(status=True, d_rel=60.0, v_lead=20.0, radar=False, model_prob=0.99) - model = make_model(30.0, desired_accel=0.0) - fill_model_lead(model.leadsV3[0], make_lead(status=True, d_rel=60.0, v_lead=32.0), 0.99) - model.leadsV3[0].v = [32.0 + 2.0 * i for i in range(len(ModelConstants.LEAD_T_IDXS))] - - vision_mpc = LongitudinalMpc() - vision_mpc.set_cur_state(30.0, 0.0) - vision_mpc.lead_v_filter.x = radar_lead.vLead - vision_traj = vision_mpc.process_lead(model.leadsV3[0], radar_lead, tracking_lead=True) - - radar_mpc = LongitudinalMpc() - radar_mpc.set_cur_state(30.0, 0.0) - radar_lead.radar = True - radar_traj = radar_mpc.process_lead(model.leadsV3[0], radar_lead, tracking_lead=True) - - assert np.allclose(vision_traj[:, 1], 20.0, atol=1e-3) - assert radar_traj[5, 1] > vision_traj[5, 1] + 1.0 diff --git a/selfdrive/test/longitudinal_maneuvers/plant.py b/selfdrive/test/longitudinal_maneuvers/plant.py index 5d0296fcb..1b688a1e9 100755 --- a/selfdrive/test/longitudinal_maneuvers/plant.py +++ b/selfdrive/test/longitudinal_maneuvers/plant.py @@ -168,25 +168,6 @@ class Plant: acceleration = log.XYZTData.new_message() acceleration.x = [float(x) for x in np.zeros_like(ModelConstants.T_IDXS)] model.modelV2.acceleration = acceleration - model.modelV2.init('leadsV3', 3) - - def fill_model_lead(lead_msg, d_rel_now, v_lead_now, a_lead_now, prob): - 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(d_rel_now + v_lead_now * 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(v_lead_now)] * len(ModelConstants.LEAD_T_IDXS) - lead_msg.vStd = [0.5] * len(ModelConstants.LEAD_T_IDXS) - lead_msg.a = [float(a_lead_now)] * len(ModelConstants.LEAD_T_IDXS) - lead_msg.aStd = [0.5] * len(ModelConstants.LEAD_T_IDXS) - - lead0_prob = 0.0 if self.only_lead2 else prob_lead - fill_model_lead(model.modelV2.leadsV3[0], d_rel, v_lead, a_lead, lead0_prob) - fill_model_lead(model.modelV2.leadsV3[1], d_rel, v_lead, a_lead, prob_lead) - fill_model_lead(model.modelV2.leadsV3[2], 200.0, self.speed, 0.0, 0.0) model.modelV2.meta.disengagePredictions.gasPressProbs = [float(prob_throttle) for _ in range(6)] control.controlsState.longControlState = LongCtrlState.pid if self.enabled else LongCtrlState.off