mawei/StarPilot
1
0
Fork 0
mirror of https://github.com/firestar5683/StarPilot.git synced 2026-06-28 01:52:06 +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
cereal/log.capnp
+6
View File
@@ -1262,6 +1262,12 @@ 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;
selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py
+35 -11
View File
@@ -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
from openpilot.selfdrive.modeld.constants import index_function, ModelConstants
if __name__ == '__main__': # generating code
from openpilot.third_party.acados.acados_template import AcadosModel, AcadosOcp, AcadosOcpSolver
@@ -30,6 +30,7 @@ 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
@@ -349,6 +350,8 @@ 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):
@@ -484,13 +487,30 @@ class LongitudinalMpc:
lead_xv = np.column_stack((x_lead_traj, v_lead_traj))
return lead_xv
def process_lead(self, lead, tracking_lead=True):
def process_lead(self, model_lead, radar_lead, tracking_lead=True):
v_ego = self.x0[1]
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
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 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
else:
# Fake a fast lead car, so mpc can keep running in the same mode
x_lead = 50.0
@@ -518,15 +538,18 @@ class LongitudinalMpc:
self.cruise_min_a = min_a
self.max_a = max_a
def update(self, radarstate, v_cruise, x, v, a, j, danger_factor, t_follow,
def update(self, radarstate, modelV2, 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(lead_one, tracking_lead)
lead_xv_1 = self.process_lead(lead_two, tracking_lead)
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
# 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
@@ -590,8 +613,9 @@ 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.modelProb > 0.9):
lead_one_prob > 0.9):
self.crash_cnt += 1
else:
self.crash_cnt = 0
selfdrive/controls/lib/longitudinal_planner.py
+8 -2
View File
@@ -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'], v_cruise, x, v, a, j,
self.mpc.update(sm['radarState'], sm['modelV2'], v_cruise, x, v, a, j,
sm['starpilotPlan'].dangerFactor, sm['starpilotPlan'].tFollow,
personality=personality, tracking_lead=lead_control_active)
@@ -569,10 +569,16 @@ class LongitudinalPlanner:
longitudinalPlan.accels = self.a_desired_trajectory.tolist()
longitudinalPlan.jerks = self.j_desired_trajectory.tolist()
longitudinalPlan.hasLead = sm['radarState'].leadOne.status
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.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
selfdrive/controls/radard.py
+44 -13
View File
@@ -19,6 +19,10 @@ 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
# radar tracks
SPEED, ACCEL = 0, 1 # Kalman filter states enum
@@ -157,16 +161,19 @@ 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,
lead_bias: float = 0.0):
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_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)),
"vRel": float(lead_v_rel_pred),
"vLead": float(v_lead),
"vLeadK": float(v_lead),
"aLeadK": blended_aLeadK,
"aLeadTau": 0.3,
"fcw": False,
@@ -178,13 +185,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, model_data: capnp._DynamicStructReader, standstill: bool,
starpilot_plan: capnp._DynamicStructReader, starpilot_toggles: SimpleNamespace,
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,
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_msg.prob > lead_detection_probability:
if len(tracks) > 0 and ready and lead_prob > lead_detection_probability:
track = match_vision_to_track(v_ego, lead_msg, model_data, tracks, starpilot_toggles)
else:
track = None
@@ -192,8 +199,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_msg.prob > lead_detection_probability):
lead_dict = get_RadarState_from_vision(lead_msg, v_ego, model_v_ego)
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)
if low_speed_override:
low_speed_tracks = [c for c in tracks.values() if c.potential_low_speed_lead(v_ego)]
@@ -230,6 +237,12 @@ 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_hist_bias = deque(maxlen=self._bias_fd_k + 1)
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)
@@ -285,10 +298,28 @@ class RadarD:
leads_v3 = sm['modelV2'].leadsV3
if len(leads_v3) > 1:
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)
lead_probs = [self.lead_prob_filters[i].update(leads_v3[i].prob) for i in range(2)]
self.vego_hist_bias.append(self.v_ego)
lead_biases = [0.0, 0.0]
for i in range(2):
lead_x = float(leads_v3[i].x[0]) if len(leads_v3[i].x) else 0.0
self.lead_drel_hists[i].append(lead_x)
if len(self.lead_drel_hists[i]) == self._bias_fd_k + 1 and len(self.vego_hist_bias) == self._bias_fd_k + 1:
dt_win = _BIAS_FD_WINDOW_S
v_ego_avg = sum(self.vego_hist_bias) / len(self.vego_hist_bias)
a_ego_win = (self.vego_hist_bias[-1] - self.vego_hist_bias[0]) / dt_win
fd_vlead = v_ego_avg + (self.lead_drel_hists[i][-1] - self.lead_drel_hists[i][0]) / dt_win + a_ego_win * dt_win / 2
bias_raw = float(leads_v3[i].v[0]) - fd_vlead
self.bias_ema_filters[i].update(bias_raw)
lead_biases[i] = self.bias_ema_filters[i].x
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)
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_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),
selfdrive/test/longitudinal_maneuvers/plant.py
+19
View File
@@ -168,6 +168,25 @@ 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