mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-08-17 14:23:45 +08:00
YOU WERE THE CHOSEN ONE
This commit is contained in:
firestar5683
@@ -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;
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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)
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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
|
||||
|
||||
Reference in New Issue
Block a user