diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 308d31389..525a71910 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -467,6 +467,7 @@ class LongitudinalPlanner: classic_model = bool(getattr(starpilot_toggles, "classic_model", False)) tinygrad_model = bool(getattr(starpilot_toggles, "tinygrad_model", False)) + experimental_mlsim = bool(tinygrad_model and self.mlsim and self.mode != 'acc') action_t = self.CP.longitudinalActuatorDelay + DT_MDL if classic_model: @@ -490,10 +491,12 @@ class LongitudinalPlanner: self.v_desired_trajectory, self.a_desired_trajectory, action_t=action_t, vEgoStopping=starpilot_toggles.vEgoStopping) + output_accel_min = get_vehicle_min_accel(self.CP, v_ego) if experimental_mlsim else accel_limits_turns[0] + close_lead_caps = [] if tracking_lead: for lead in (self.lead_one, self.lead_two): - cap = self.get_close_lead_brake_cap(lead, v_ego, accel_limits_turns[0]) + cap = self.get_close_lead_brake_cap(lead, v_ego, output_accel_min) if cap is not None: close_lead_caps.append(cap) if close_lead_caps: @@ -514,7 +517,7 @@ class LongitudinalPlanner: output_a_target = min(output_a_target, cruise_accel_cap) output_accel_max = no_throttle_output_max if not self.allow_throttle else accel_limits_turns[1] - output_a_target = float(np.clip(output_a_target, accel_limits_turns[0], output_accel_max)) + output_a_target = float(np.clip(output_a_target, output_accel_min, output_accel_max)) self.output_a_target = output_a_target self.output_should_stop = bool(output_should_stop) diff --git a/selfdrive/controls/tests/test_conditional_experimental_mode.py b/selfdrive/controls/tests/test_conditional_experimental_mode.py index 34c93cce2..67671ced0 100644 --- a/selfdrive/controls/tests/test_conditional_experimental_mode.py +++ b/selfdrive/controls/tests/test_conditional_experimental_mode.py @@ -1,6 +1,9 @@ +from pathlib import Path from types import SimpleNamespace from openpilot.common.constants import CV +from openpilot.starpilot.controls.starpilot_planner import StarPilotPlanner +import openpilot.starpilot.controls.starpilot_planner as starpilot_planner_module from openpilot.starpilot.controls.lib.conditional_experimental_mode import ConditionalExperimentalMode @@ -39,3 +42,79 @@ def test_predicted_stop_within_threshold_triggers_stop_light(): cem.stop_sign_and_light(v_ego, make_sm(), model_time=7.0) assert cem.stop_light_detected + + +class DummyThemeManager: + def update_wheel_image(self, *args, **kwargs): + pass + + +def test_starpilot_planner_updates_cem_with_current_frame_state(monkeypatch): + planner = StarPilotPlanner(Path("/tmp/nonexistent"), DummyThemeManager()) + + try: + monkeypatch.setattr(starpilot_planner_module, "calculate_road_curvature", lambda model, v_ego: (0.01, 1.0)) + monkeypatch.setattr(planner.starpilot_acceleration, "update", lambda *args, **kwargs: None) + monkeypatch.setattr(planner.starpilot_events, "update", lambda *args, **kwargs: None) + monkeypatch.setattr(planner.starpilot_vcruise, "update", lambda *args, **kwargs: 0.0) + monkeypatch.setattr(planner.starpilot_weather, "update_weather", lambda *args, **kwargs: None) + + def following_update(*args, **kwargs): + planner.starpilot_following.following_lead = planner.tracking_lead + planner.starpilot_following.slower_lead = planner.tracking_lead + + monkeypatch.setattr(planner.starpilot_following, "update", following_update) + + seen = {} + + def cem_update(v_ego, sm, starpilot_toggles): + seen.update({ + "tracking_lead": planner.tracking_lead, + "following_lead": planner.starpilot_following.following_lead, + "slower_lead": planner.starpilot_following.slower_lead, + "model_length": planner.model_length, + "driving_in_curve": planner.driving_in_curve, + "road_curvature_detected": planner.road_curvature_detected, + }) + + monkeypatch.setattr(planner.starpilot_cem, "update", cem_update) + + planner.model_length = 0.0 + planner.tracking_lead = False + planner.driving_in_curve = False + planner.lateral_acceleration = 0.0 + planner.road_curvature_detected = False + planner.starpilot_following.following_lead = False + planner.starpilot_following.slower_lead = False + + sm = { + "radarState": SimpleNamespace(leadOne=SimpleNamespace(status=True, dRel=25.0, vLead=0.5)), + "selfdriveState": SimpleNamespace(enabled=True), + "carState": SimpleNamespace(vCruise=50.0, vEgo=20.0, standstill=False, leftBlinker=False, rightBlinker=False), + "controlsState": SimpleNamespace(curvature=0.02), + "modelV2": SimpleNamespace(position=SimpleNamespace(x=[0.0, 30.0]), laneLines=[None] * 4, roadEdges=[None] * 2), + "starpilotCarState": SimpleNamespace(pauseLateral=False), + planner.gps_location_service: SimpleNamespace(latitude=1.0, longitude=1.0, bearingDeg=90.0), + } + starpilot_toggles = SimpleNamespace( + set_speed_offset=0, + conditional_experimental_mode=True, + minimum_lane_change_speed=100.0, + pause_lateral_below_speed=0.0, + pause_lateral_below_signal=False, + stop_distance=6.0, + weather_presets=False, + ) + + planner.update(0.0, False, sm, starpilot_toggles) + + assert seen == { + "tracking_lead": True, + "following_lead": True, + "slower_lead": True, + "model_length": 30.0, + "driving_in_curve": True, + "road_curvature_detected": True, + } + finally: + planner.shutdown() diff --git a/selfdrive/controls/tests/test_longitudinal_planner.py b/selfdrive/controls/tests/test_longitudinal_planner.py new file mode 100644 index 000000000..fa5480cad --- /dev/null +++ b/selfdrive/controls/tests/test_longitudinal_planner.py @@ -0,0 +1,117 @@ +from types import SimpleNamespace + +import pytest + +from cereal import log +from opendbc.car.honda.interface import CarInterface +from opendbc.car.honda.values import CAR +from openpilot.selfdrive.controls.lib.longcontrol import LongCtrlState +from openpilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlanner, get_vehicle_min_accel +from openpilot.selfdrive.modeld.constants import ModelConstants + + +def make_lead(*, status: bool, d_rel: float = 200.0, v_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 = 0.0 + lead.vRel = 0.0 + lead.aRel = 0.0 + lead.modelProb = 0.0 + return lead + + +def make_model(v_ego: float, desired_accel: float): + model = log.ModelDataV2.new_message() + t_idxs = ModelConstants.T_IDXS + + model.position.x = [float(v_ego * t) for t in t_idxs] + model.position.y = [0.0] * len(t_idxs) + model.position.z = [0.0] * len(t_idxs) + model.position.t = [float(t) for t in t_idxs] + + model.velocity.x = [float(v_ego)] * len(t_idxs) + model.velocity.y = [0.0] * len(t_idxs) + model.velocity.z = [0.0] * len(t_idxs) + model.velocity.t = [float(t) for t in t_idxs] + + model.acceleration.x = [0.0] * len(t_idxs) + model.acceleration.y = [0.0] * len(t_idxs) + model.acceleration.z = [0.0] * len(t_idxs) + model.acceleration.t = [float(t) for t in t_idxs] + + model.meta.disengagePredictions.gasPressProbs = [1.0] * 6 + model.action.desiredAcceleration = desired_accel + model.action.shouldStop = False + return model + + +def make_sm(v_ego: float, desired_accel: float, min_accel: float): + return { + "carControl": SimpleNamespace(orientationNED=[0.0, 0.0, 0.0]), + "carState": SimpleNamespace( + vEgo=v_ego, + vEgoCluster=v_ego, + aEgo=0.0, + vCruise=100.0, + standstill=False, + steeringAngleDeg=0.0, + ), + "controlsState": SimpleNamespace( + longControlState=LongCtrlState.pid, + forceDecel=False, + ), + "liveParameters": SimpleNamespace(angleOffsetDeg=0.0), + "modelV2": make_model(v_ego, desired_accel), + "radarState": SimpleNamespace( + leadOne=make_lead(status=False), + leadTwo=make_lead(status=False), + ), + "selfdriveState": SimpleNamespace(enabled=True, experimentalMode=True, personality=0), + "starpilotPlan": SimpleNamespace( + vCruise=v_ego + 5.0, + minAcceleration=min_accel, + maxAcceleration=2.0, + disableThrottle=False, + trackingLead=False, + accelerationJerk=5.0, + dangerJerk=5.0, + speedJerk=5.0, + dangerFactor=1.0, + tFollow=1.45, + forcingStopLength=2, + ), + } + + +def make_toggles(): + return SimpleNamespace( + taco_tune=False, + classic_model=False, + tinygrad_model=True, + model_version="v11", + stop_distance=6.0, + vEgoStopping=0.5, + ) + + +def test_experimental_mlsim_uses_vehicle_min_accel_floor(): + v_ego = 18.0 + desired_accel = -1.0 + comfort_min_accel = -0.5 + + CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) + planner = LongitudinalPlanner(CP, init_v=v_ego) + sm = make_sm(v_ego, desired_accel, comfort_min_accel) + + vehicle_min_accel = get_vehicle_min_accel(CP, v_ego) + assert vehicle_min_accel < comfort_min_accel + + planner.update(sm, make_toggles()) + + assert planner.mode == "blended" + assert planner.mlsim + assert planner.output_a_target == pytest.approx(desired_accel, abs=1e-3) + assert planner.output_a_target < comfort_min_accel diff --git a/starpilot/controls/starpilot_planner.py b/starpilot/controls/starpilot_planner.py index 77024daaf..574f7cc6e 100644 --- a/starpilot/controls/starpilot_planner.py +++ b/starpilot/controls/starpilot_planner.py @@ -86,18 +86,6 @@ class StarPilotPlanner: self.starpilot_acceleration.max_accel = 0 self.starpilot_acceleration.min_accel = 0 - if controls_enabled and starpilot_toggles.conditional_experimental_mode: - self.starpilot_cem.update(v_ego, sm, starpilot_toggles) - else: - self.starpilot_cem.curve_detected = False - self.starpilot_cem.stop_sign_and_light(v_ego, sm, PLANNER_TIME - 2) - - self.driving_in_curve = abs(self.lateral_acceleration) >= MINIMUM_LATERAL_ACCELERATION - - self.starpilot_events.update(controls_enabled, v_cruise, sm, starpilot_toggles) - - self.starpilot_following.update(controls_enabled, v_ego, sm, starpilot_toggles) - gps_location = sm[self.gps_location_service] self.gps_position = { "latitude": gps_location.latitude, @@ -115,6 +103,7 @@ class StarPilotPlanner: self.lane_width_right = 0 self.lateral_acceleration = v_ego**2 * sm["controlsState"].curvature + self.driving_in_curve = abs(self.lateral_acceleration) >= MINIMUM_LATERAL_ACCELERATION self.lateral_check = v_ego >= starpilot_toggles.pause_lateral_below_speed self.lateral_check |= not (sm["carState"].leftBlinker or sm["carState"].rightBlinker) and starpilot_toggles.pause_lateral_below_signal @@ -133,8 +122,18 @@ class StarPilotPlanner: if not sm["carState"].standstill: self.tracking_lead = self.update_lead_status(starpilot_toggles.stop_distance) + self.starpilot_following.update(controls_enabled, v_ego, sm, starpilot_toggles) + + if controls_enabled and starpilot_toggles.conditional_experimental_mode: + self.starpilot_cem.update(v_ego, sm, starpilot_toggles) + else: + self.starpilot_cem.curve_detected = False + self.starpilot_cem.stop_sign_and_light(v_ego, sm, PLANNER_TIME - 2) + self.v_cruise = self.starpilot_vcruise.update(controls_enabled, now, time_validated, v_cruise, v_ego, sm, starpilot_toggles) + self.starpilot_events.update(controls_enabled, v_cruise, sm, starpilot_toggles) + if self.gps_valid and time_validated and starpilot_toggles.weather_presets: self.starpilot_weather.update_weather(now, starpilot_toggles) else: