diff --git a/common/params_keys.h b/common/params_keys.h index 3b651a483..27646b56e 100644 --- a/common/params_keys.h +++ b/common/params_keys.h @@ -371,6 +371,9 @@ inline static std::unordered_map keys = { {"IssueReported", {CLEAR_ON_MANAGER_START, JSON, "{}", "{}"}}, {"KonikDongleId", {PERSISTENT, STRING, "", "", 0}}, {"KonikMinutes", {PERSISTENT, INT, "0", "0", 0}}, + {"LaneCentering", {PERSISTENT, BOOL, "0", "0", 2}}, + {"LaneCenteringE2EAuthority", {PERSISTENT, FLOAT, "1.0", "1.0", 3}}, + {"LaneCenterOffset", {PERSISTENT, FLOAT, "0.0", "0.0", 3}}, {"LaneChanges", {PERSISTENT, BOOL, "1", "1", 0, SETTINGS_SIMPLE}}, {"LaneChangeSmoothing", {PERSISTENT, INT, "5", "10", 1, SETTINGS_SIMPLE}}, {"LaneChangeTime", {PERSISTENT, FLOAT, "1.0", "0.0", 1, SETTINGS_SIMPLE}}, diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 2215105e8..0738d3fbc 100644 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -14,6 +14,7 @@ from opendbc.car.chrysler.values import pacifica_hybrid_aol_stock_acc_mode from opendbc.car.gm.values import CAR as GM_CAR from opendbc.car.vehicle_model import VehicleModel from openpilot.selfdrive.controls.lib.drive_helpers import MAX_LATERAL_JERK, clip_curvature, get_lateral_active +from openpilot.selfdrive.controls.lib.lane_centering import LaneCenteringController from openpilot.selfdrive.controls.lib.latcontrol import LatControl from openpilot.selfdrive.controls.lib.latcontrol_pid import LatControlPID from openpilot.selfdrive.controls.lib.latcontrol_angle import LatControlAngle, STEER_ANGLE_SATURATION_THRESHOLD @@ -305,6 +306,7 @@ class Controls: self.curvature = 0.0 self.desired_curvature = 0.0 self.lc_smooth_release = 0.0 + self.lane_centering = LaneCenteringController() self.lc_entry_sign = 0.0 self.lc_arrest_jerk_factor = 1.0 self.turn_hold_curvature = 0.0 @@ -419,6 +421,7 @@ class Controls: if not CC.latActive: self.LaC.reset() + self.lane_centering.reset() if not CC.longActive: self.LoC.reset() @@ -585,6 +588,14 @@ class Controls: held_mag = min(lead_curvature * blinker_dir, abs(self.turn_hold_curvature) + CURVATURE_HOLD_RATCHET_RATE * DT_CTRL) self.turn_hold_curvature = math.copysign(held_mag, lead_curvature) + new_desired_curvature = self.lane_centering.update( + new_desired_curvature, model_v2, CS.vEgo, + self.starpilot_toggles.lane_centering, + self.starpilot_toggles.lane_center_offset, + self.starpilot_toggles.lane_centering_e2e_authority, + CC.latActive, + bool(self.sm.all_checks(['modelV2']))) + jerk_factor = 1.0 if self.starpilot_toggles.lane_change_pace < 10: set_jerk = self.starpilot_toggles.lane_change_jerk_factor diff --git a/selfdrive/controls/lib/lane_centering.py b/selfdrive/controls/lib/lane_centering.py new file mode 100644 index 000000000..c4b7344e2 --- /dev/null +++ b/selfdrive/controls/lib/lane_centering.py @@ -0,0 +1,134 @@ +from cereal import log +import numpy as np + +from openpilot.common.realtime import DT_CTRL +from openpilot.selfdrive.controls.lib.drive_helpers import smooth_value + + +_MIN_V_EGO = 5.0 +_MIN_LANE_PROB = 0.6 +_MAX_LANE_STD = 0.3 +_MIN_LANE_WIDTH = 2.6 +_MAX_LANE_WIDTH = 4.8 +_MAX_OFFSET = 0.3 +_MIN_CENTER_TO_LINE = 1.1 +_MAX_RAW_CORRECTION = 0.004 +_MAX_GAIN = 0.30 +_SMOOTH_TAU = 0.4 + +_E2E_MAX_PATH_STD = 0.35 +_E2E_BREAK_IN_START = 0.25 +_E2E_BREAK_IN_FULL = 0.75 + + +class LaneCenteringController: + def __init__(self) -> None: + self._correction = 0.0 + + def reset(self) -> None: + self._correction = 0.0 + + def update(self, model_curvature, model_v2, v_ego, enabled, offset, e2e_authority, lat_active, model_valid) -> float: + model_curvature = float(model_curvature) + + try: + v_ego = float(v_ego) + offset = float(offset) + e2e_authority = float(e2e_authority) + except (TypeError, ValueError): + self.reset() + return model_curvature + + if not np.isfinite([v_ego, offset, e2e_authority]).all(): + self.reset() + return model_curvature + + if not model_valid or not enabled or not lat_active or v_ego < _MIN_V_EGO: + self.reset() + return model_curvature + + try: + if model_v2.meta.laneChangeState != log.LaneChangeState.off: + self.reset() + return model_curvature + except (AttributeError, TypeError, ValueError): + self.reset() + return model_curvature + + valid, raw_correction = self._raw_correction( + model_v2, + v_ego, + float(np.clip(offset, -_MAX_OFFSET, _MAX_OFFSET)), + float(np.clip(e2e_authority, 0.0, 1.0)), + ) + if not valid: + self.reset() + return model_curvature + + target = float(np.clip(raw_correction, -_MAX_RAW_CORRECTION, _MAX_RAW_CORRECTION)) * _MAX_GAIN + self._correction = float(smooth_value(target, self._correction, _SMOOTH_TAU, dt=DT_CTRL)) + return model_curvature + self._correction + + @staticmethod + def _valid_path(x, y) -> bool: + return x.size >= 2 and x.size == y.size and np.isfinite(x).all() and np.isfinite(y).all() and np.all(np.diff(x) > 0) + + @staticmethod + def _covers(x, distance: float) -> bool: + return bool(x[0] <= distance <= x[-1]) + + def _raw_correction(self, model_v2, v_ego: float, offset: float, e2e_authority: float) -> tuple[bool, float]: + try: + lane_lines = model_v2.laneLines + probs = np.asarray(model_v2.laneLineProbs, dtype=float) + stds = np.asarray(model_v2.laneLineStds, dtype=float) + if len(lane_lines) < 3 or probs.size < 3 or stds.size < 3: + return False, 0.0 + if not np.isfinite(probs[[1, 2]]).all() or not np.isfinite(stds[[1, 2]]).all(): + return False, 0.0 + if np.any(probs[[1, 2]] < _MIN_LANE_PROB) or np.any(probs[[1, 2]] > 1.0): + return False, 0.0 + if np.any(stds[[1, 2]] < 0.0) or np.any(stds[[1, 2]] > _MAX_LANE_STD): + return False, 0.0 + + left_x = np.asarray(lane_lines[1].x, dtype=float) + left_y = np.asarray(lane_lines[1].y, dtype=float) + right_x = np.asarray(lane_lines[2].x, dtype=float) + right_y = np.asarray(lane_lines[2].y, dtype=float) + pos_x = np.asarray(model_v2.position.x, dtype=float) + pos_y = np.asarray(model_v2.position.y, dtype=float) + if not (self._valid_path(left_x, left_y) and self._valid_path(right_x, right_y) and self._valid_path(pos_x, pos_y)): + return False, 0.0 + + lookahead = float(np.clip(v_ego, 8.0, 35.0)) + if not all(self._covers(x, lookahead) for x in (left_x, right_x, pos_x)): + return False, 0.0 + + left = float(np.interp(lookahead, left_x, left_y)) + right = float(np.interp(lookahead, right_x, right_y)) + width = right - left + if not _MIN_LANE_WIDTH <= width <= _MAX_LANE_WIDTH: + return False, 0.0 + + max_safe_offset = min(_MAX_OFFSET, max(0.0, width * 0.5 - _MIN_CENTER_TO_LINE)) + target_y = 0.5 * (left + right) + float(np.clip(offset, -max_safe_offset, max_safe_offset)) + model_y = float(np.interp(lookahead, pos_x, pos_y)) + error = target_y - model_y + + try: + pos_y_std = np.asarray(model_v2.position.yStd, dtype=float) + if self._valid_path(pos_x, pos_y_std): + path_std = float(np.interp(lookahead, pos_x, pos_y_std)) + if 0.0 <= path_std <= _E2E_MAX_PATH_STD: + break_in = np.clip( + (abs(error) - _E2E_BREAK_IN_START) / (_E2E_BREAK_IN_FULL - _E2E_BREAK_IN_START), + 0.0, + 1.0, + ) + error *= 1.0 - e2e_authority * float(break_in) + except (AttributeError, TypeError, ValueError): + pass + + return True, float(2.0 * error / lookahead ** 2) + except (AttributeError, IndexError, TypeError, ValueError): + return False, 0.0 diff --git a/selfdrive/controls/tests/test_lane_centering.py b/selfdrive/controls/tests/test_lane_centering.py new file mode 100644 index 000000000..ecdfd4d31 --- /dev/null +++ b/selfdrive/controls/tests/test_lane_centering.py @@ -0,0 +1,138 @@ +from types import SimpleNamespace + +import numpy as np +import pytest + +from openpilot.selfdrive.controls.lib.lane_centering import LaneCenteringController + + +_V_EGO = 20.0 +_XS = np.linspace(0.0, 50.0, 52) + + +def _path(y, y_std=0.1): + return SimpleNamespace( + x=_XS.copy(), + y=np.full_like(_XS, float(y)), + yStd=np.full_like(_XS, float(y_std)), + ) + + +def _model(left=-1.8, right=1.8, model_y=0.0, lane_prob=0.9, lane_std=0.1, path_std=0.1, lane_change=0): + return SimpleNamespace( + laneLines=[_path(0.0), _path(left), _path(right), _path(0.0)], + laneLineProbs=[0.0, lane_prob, lane_prob, 0.0], + laneLineStds=[0.0, lane_std, lane_std, 0.0], + position=_path(model_y, path_std), + meta=SimpleNamespace(laneChangeState=lane_change), + ) + + +def _update(controller, model, *, offset=0.0, authority=1.0, enabled=True, active=True, valid=True, speed=_V_EGO): + return controller.update(0.0, model, speed, enabled, offset, authority, active, valid) + + +def _converge(model, *, offset=0.0, authority=1.0): + controller = LaneCenteringController() + output = 0.0 + for _ in range(300): + output = _update(controller, model, offset=offset, authority=authority) + return controller, output + + +@pytest.mark.parametrize( + "kwargs", + [ + {"enabled": False}, + {"active": False}, + {"valid": False}, + {"speed": 4.9}, + ], +) +def test_hard_gates_are_noop(kwargs): + assert _update(LaneCenteringController(), _model(left=-1.5, right=2.1), **kwargs) == 0.0 + + +def test_lane_change_is_noop(): + assert _update(LaneCenteringController(), _model(left=-1.5, right=2.1, lane_change=1)) == 0.0 + + +@pytest.mark.parametrize( + "field,value", + [ + ("prob", np.nan), + ("prob", 1.1), + ("std", np.nan), + ("std", -0.1), + ], +) +def test_invalid_lane_confidence_is_rejected(field, value): + model = _model(left=-1.5, right=2.1) + values = model.laneLineProbs if field == "prob" else model.laneLineStds + values[1] = value + assert _update(LaneCenteringController(), model) == 0.0 + + +def test_input_must_cover_lookahead(): + model = _model(left=-1.5, right=2.1) + model.laneLines[1].x = model.laneLines[1].x[:10] + model.laneLines[1].y = model.laneLines[1].y[:10] + assert _update(LaneCenteringController(), model) == 0.0 + + +def test_lane_center_error_steers_toward_center(): + _, right = _converge(_model(left=-1.5, right=2.1), authority=0.0) + _, left = _converge(_model(left=-2.1, right=1.5), authority=0.0) + assert right > 0.0 + assert left < 0.0 + + +def test_offset_direction(): + _, right = _converge(_model(), offset=0.2, authority=0.0) + _, left = _converge(_model(), offset=-0.2, authority=0.0) + assert right > 0.0 + assert left < 0.0 + + +def test_offset_is_reduced_in_narrow_lane(): + narrow = _model(left=-1.3, right=1.3) + _, at_safe_limit = _converge(narrow, offset=0.2, authority=0.0) + _, above_safe_limit = _converge(narrow, offset=0.3, authority=0.0) + assert np.isclose(at_safe_limit, above_safe_limit) + + +def test_confident_e2e_path_can_fully_break_in(): + model = _model(left=-1.0, right=2.6, model_y=0.0, path_std=0.1) + _, lane_authority = _converge(model, authority=0.0) + _, e2e_authority = _converge(model, authority=1.0) + assert lane_authority > 0.0 + assert abs(e2e_authority) < 1e-9 + + +def test_uncertain_e2e_path_does_not_break_in(): + model = _model(left=-1.0, right=2.6, model_y=0.0, path_std=0.6) + _, output = _converge(model, authority=1.0) + assert output > 0.0 + + +def test_e2e_authority_blends_lane_correction(): + model = _model(left=-1.2, right=2.4, model_y=0.0, path_std=0.1) + _, lane_only = _converge(model, authority=0.0) + _, blended = _converge(model, authority=0.5) + _, e2e = _converge(model, authority=1.0) + assert lane_only > blended > e2e >= 0.0 + + +def test_confidence_loss_drops_filtered_correction(): + controller, output = _converge(_model(left=-1.5, right=2.1), authority=0.0) + assert output > 0.0 + assert _update(controller, _model(left=-1.5, right=2.1, lane_prob=0.2), authority=0.0) == 0.0 + + +def test_correction_is_smoothed_and_capped(): + controller = LaneCenteringController() + model = _model(left=0.0, right=3.0, path_std=0.6) + first = _update(controller, model, authority=0.0) + _, steady = _converge(model, authority=0.0) + assert 0.0 < first < steady + assert np.isclose(steady, 0.004 * 0.30, atol=1e-6) diff --git a/starpilot/common/safe_mode.py b/starpilot/common/safe_mode.py index 3598adde0..10b8902b9 100644 --- a/starpilot/common/safe_mode.py +++ b/starpilot/common/safe_mode.py @@ -33,6 +33,10 @@ SAFE_MODE_MANAGED_KEYS = ( "SteerKP", "SteerLatAccel", "SteerRatio", + "CameraOffset", + "LaneCentering", + "LaneCenteringE2EAuthority", + "LaneCenterOffset", "LaneChanges", "LaneChangeTime", "LaneDetectionWidth", diff --git a/starpilot/common/starpilot_variables.py b/starpilot/common/starpilot_variables.py index 3dbb6c9bb..a72ee2ce4 100644 --- a/starpilot/common/starpilot_variables.py +++ b/starpilot/common/starpilot_variables.py @@ -705,6 +705,7 @@ class StarPilotVariables: toggle.use_wheel_speed = self.get_value("WheelSpeed", condition=advanced_custom_ui) advanced_lateral_tuning = self.get_value("AdvancedLateralTune") + toggle.lane_centering = self.get_value("LaneCentering") toggle.force_auto_tune = self.get_value("ForceAutoTune", condition=advanced_lateral_tuning and not has_auto_tune and is_torque_car and not is_angle_car) # Force-off is also meaningful on manually tuned torque cars: it locks the # vehicle-model parameters instead of allowing paramsd to learn over them. @@ -729,6 +730,11 @@ class StarPilotVariables: honda_pid_lateral = toggle.car_make == "honda" and CP.lateralTuning.which() == "pid" and not is_angle_car toggle.honda_lateral_pid_kp_scale = self.get_value("HondaLateralPidKpScale", cast=float, condition=honda_pid_lateral, default=1.0, min=0.1, max=4.0) toggle.honda_lateral_pid_ki_scale = self.get_value("HondaLateralPidKiScale", cast=float, condition=honda_pid_lateral, default=1.0, min=0.1, max=4.0) + toggle.lane_center_offset = self.get_value("LaneCenterOffset", cast=float, condition=toggle.lane_centering, default=0.0, min=-0.3, max=0.3) + toggle.lane_centering_e2e_authority = self.get_value( + "LaneCenteringE2EAuthority", cast=float, condition=toggle.lane_centering, + default=1.0, min=0.0, max=1.0, + ) advanced_longitudinal_tuning = toggle.openpilot_longitudinal and self.get_value("AdvancedLongitudinalTune") ev_vehicle = default_ev_tuning_enabled(CP) diff --git a/starpilot/common/tests/test_lane_centering_galaxy.py b/starpilot/common/tests/test_lane_centering_galaxy.py new file mode 100644 index 000000000..51249d413 --- /dev/null +++ b/starpilot/common/tests/test_lane_centering_galaxy.py @@ -0,0 +1,46 @@ +import json +from pathlib import Path + + +_LAYOUT_PATH = Path(__file__).resolve().parents[2] / "system/the_galaxy/assets/components/tools/device_settings_layout.json" + + +def _sections(): + layout = json.loads(_LAYOUT_PATH.read_text(encoding="utf-8")) + return { + section["name"]: {param["key"]: param for param in section["params"]} + for section in layout + } + + +def test_lane_centering_is_only_in_galaxy_developer_section(): + sections = _sections() + keys = {"LaneCentering", "LaneCenterOffset", "LaneCenteringE2EAuthority"} + + assert keys <= sections["Developer"].keys() + for name, params in sections.items(): + if name != "Developer": + assert keys.isdisjoint(params) + + for key in keys: + assert sections["Developer"][key]["settings_tier"] == "advanced" + + +def test_lane_centering_galaxy_controls(): + developer = _sections()["Developer"] + centering = developer["LaneCentering"] + offset = developer["LaneCenterOffset"] + e2e_authority = developer["LaneCenteringE2EAuthority"] + + assert centering["ui_type"] == "toggle" + assert centering["is_parent_toggle"] is True + + assert offset["parent_key"] == "LaneCentering" + assert offset["min"] == -0.3 + assert offset["max"] == 0.3 + assert offset["step"] == 0.01 + + assert e2e_authority["parent_key"] == "LaneCentering" + assert e2e_authority["min"] == 0.0 + assert e2e_authority["max"] == 1.0 + assert e2e_authority["step"] == 0.05 diff --git a/starpilot/common/tests/test_safe_mode_lane_centering.py b/starpilot/common/tests/test_safe_mode_lane_centering.py new file mode 100644 index 000000000..adf3db6d0 --- /dev/null +++ b/starpilot/common/tests/test_safe_mode_lane_centering.py @@ -0,0 +1,10 @@ +from openpilot.starpilot.common.safe_mode import SAFE_MODE_MANAGED_KEYS + + +def test_safe_mode_manages_lane_centering_settings(): + assert { + "CameraOffset", + "LaneCentering", + "LaneCenteringE2EAuthority", + "LaneCenterOffset", + } <= set(SAFE_MODE_MANAGED_KEYS) diff --git a/starpilot/system/the_galaxy/assets/components/tools/device_settings_layout.json b/starpilot/system/the_galaxy/assets/components/tools/device_settings_layout.json index 4e5de9493..6cd4ed263 100644 --- a/starpilot/system/the_galaxy/assets/components/tools/device_settings_layout.json +++ b/starpilot/system/the_galaxy/assets/components/tools/device_settings_layout.json @@ -3972,6 +3972,41 @@ "precision": 2, "settings_tier": "advanced" }, + { + "key": "LaneCentering", + "label": "Lane Centering", + "description": "Experimentally bias the model command toward the detected lane center. Requires two confident lane lines and remains subject to normal curvature and jerk limits.", + "data_type": "bool", + "ui_type": "toggle", + "is_parent_toggle": true, + "settings_tier": "advanced" + }, + { + "key": "LaneCenterOffset", + "label": "Lane Center Offset", + "description": "Shift the lane target left or right in meters. The controller reduces this offset automatically when the detected lane is narrow.", + "data_type": "float", + "ui_type": "numeric", + "min": -0.3, + "max": 0.3, + "step": 0.01, + "precision": 2, + "parent_key": "LaneCentering", + "settings_tier": "advanced" + }, + { + "key": "LaneCenteringE2EAuthority", + "label": "E2E Break-In Authority", + "description": "How strongly a confident end-to-end path can override lane centering when it deliberately departs the lane target. 1.0 gives the model full authority; 0.0 disables break-in.", + "data_type": "float", + "ui_type": "numeric", + "min": 0.0, + "max": 1.0, + "step": 0.05, + "precision": 2, + "parent_key": "LaneCentering", + "settings_tier": "advanced" + }, { "key": "HondaLateralPidKpScale", "label": "Honda PID Kp Scale", diff --git a/starpilot/system/the_galaxy/the_galaxy.py b/starpilot/system/the_galaxy/the_galaxy.py index 9c058bbad..f2a2e7a93 100644 --- a/starpilot/system/the_galaxy/the_galaxy.py +++ b/starpilot/system/the_galaxy/the_galaxy.py @@ -959,6 +959,10 @@ _TROUBLESHOOT_ADVANCED_LATERAL_KEYS = [ "ForceAutoTune", "ForceAutoTuneOff", "ForceTorqueController", + "CameraOffset", + "LaneCentering", + "LaneCenteringE2EAuthority", + "LaneCenterOffset", ] _TROUBLESHOOT_ADVANCED_LONGITUDINAL_KEYS = [