From a96f15c194c10a18085ebbaae400cda510143373 Mon Sep 17 00:00:00 2001 From: whoisdomi Date: Thu, 23 Jul 2026 09:46:53 -0500 Subject: [PATCH] Close Gap on Lane Changes --- common/params_keys.h | 2 + .../ui/layouts/settings/starpilot/lateral.py | 20 +++++ starpilot/common/starpilot_variables.py | 2 + starpilot/controls/lib/starpilot_following.py | 74 +++++++++++++++++++ .../tools/device_settings_layout.json | 21 ++++++ 5 files changed, 119 insertions(+) diff --git a/common/params_keys.h b/common/params_keys.h index 980c9865f..751f18348 100644 --- a/common/params_keys.h +++ b/common/params_keys.h @@ -375,6 +375,8 @@ inline static std::unordered_map keys = { {"LaneCenteringE2EAuthority", {PERSISTENT, FLOAT, "1.0", "1.0", 3}}, {"LaneCenterOffset", {PERSISTENT, FLOAT, "0.0", "0.0", 3}}, {"LaneChanges", {PERSISTENT, BOOL, "1", "1", 0, SETTINGS_SIMPLE}}, + {"LaneChangeCloseGap", {PERSISTENT, BOOL, "0", "0", 1, SETTINGS_SIMPLE}}, + {"LaneChangeCloseGapSeconds", {PERSISTENT, FLOAT, "1.0", "1.0", 1, SETTINGS_SIMPLE}}, {"LaneChangeSmoothing", {PERSISTENT, INT, "5", "10", 1, SETTINGS_SIMPLE}}, {"LaneChangeTime", {PERSISTENT, FLOAT, "1.0", "0.0", 1, SETTINGS_SIMPLE}}, {"LaneDetectionWidth", {PERSISTENT, FLOAT, "0.0", "0.0", 1, SETTINGS_SIMPLE}}, diff --git a/selfdrive/ui/layouts/settings/starpilot/lateral.py b/selfdrive/ui/layouts/settings/starpilot/lateral.py index 8edecd006..26fbc9da2 100644 --- a/selfdrive/ui/layouts/settings/starpilot/lateral.py +++ b/selfdrive/ui/layouts/settings/starpilot/lateral.py @@ -108,6 +108,9 @@ class StarPilotLateralLayout(_SettingsPage): def nlc_on(): return lc_on() and p.get_bool("NudgelessLaneChange") + def close_gap_on(): + return lc_on() and p.get_bool("LaneChangeCloseGap") + def pos_on(): return p.get_bool("PauseLateralOnSignal") @@ -191,6 +194,20 @@ class StarPilotLateralLayout(_SettingsPage): on_click=self._show_lane_smoothing, visible=lc_on, ), + SettingRow( + "LaneChangeCloseGap", "toggle", tr_noop("Close Gap On Lane Change"), + subtitle=tr_noop("Allows for a temporary shorter follow distance behind lead so that openpilot merges smoothly out of current lane, it will allow car to accelerate as it changes lanes."), + get_state=lambda: p.get_bool("LaneChangeCloseGap"), + set_state=lambda s: p.put_bool("LaneChangeCloseGap", s), + visible=lc_on, + ), + SettingRow( + "LaneChangeCloseGapSeconds", "value", tr_noop("Temporary Follow Distance"), + subtitle=tr_noop("Follow distance to hold while changing lanes. Only applied when shorter than your normal gap."), + get_value=self._get_lane_change_close_gap_display, + on_click=lambda: self._show_slider("LaneChangeCloseGapSeconds", 0.5, 3.0, step=0.05, unit="s", value_type="float"), + visible=close_gap_on, + ), ] # ── 3. Advanced Lateral Tuning ── @@ -375,6 +392,9 @@ class StarPilotLateralLayout(_SettingsPage): return tr("Stock") return str(val) + def _get_lane_change_close_gap_display(self) -> str: + return f"{self._params.get_float('LaneChangeCloseGapSeconds'):.2f}s" + def _show_lane_smoothing(self): def on_close(res, val): if res == DialogResult.CONFIRM: diff --git a/starpilot/common/starpilot_variables.py b/starpilot/common/starpilot_variables.py index 68ff27724..968c3c5bb 100644 --- a/starpilot/common/starpilot_variables.py +++ b/starpilot/common/starpilot_variables.py @@ -1041,6 +1041,8 @@ class StarPilotVariables: toggle.lane_change_delay = self.get_value("LaneChangeTime", cast=float, condition=toggle.lane_changes) toggle.lane_detection_width = self.get_value("LaneDetectionWidth", cast=float, condition=toggle.lane_changes, conversion=distance_conversion) toggle.minimum_lane_change_speed = self.get_value("MinimumLaneChangeSpeed", cast=float, condition=toggle.lane_changes, conversion=speed_conversion) + toggle.lane_change_close_gap = self.get_value("LaneChangeCloseGap", condition=toggle.lane_changes) + toggle.lane_change_close_gap_seconds = self.get_value("LaneChangeCloseGapSeconds", cast=float, condition=toggle.lane_change_close_gap, min=0.5, max=3.0) toggle.nudgeless = self.get_value("NudgelessLaneChange", condition=toggle.lane_changes) toggle.nudgeless_lane_change_only_when_engaged = self.get_value("NudgelessLaneChangeOnlyWhenEngaged", condition=toggle.lane_changes and toggle.nudgeless) toggle.one_lane_change = self.get_value("OneLaneChange", condition=toggle.lane_changes) diff --git a/starpilot/controls/lib/starpilot_following.py b/starpilot/controls/lib/starpilot_following.py index df1eca5f3..cceeaf439 100644 --- a/starpilot/controls/lib/starpilot_following.py +++ b/starpilot/controls/lib/starpilot_following.py @@ -1,7 +1,9 @@ #!/usr/bin/env python3 import numpy as np +from cereal import log from openpilot.common.constants import CV +from openpilot.common.realtime import DT_MDL from openpilot.selfdrive.controls.lib.lead_behavior import should_disable_far_lead_throttle from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import COMFORT_BRAKE, LEAD_DANGER_FACTOR, desired_follow_distance, get_jerk_factor, get_T_FOLLOW @@ -10,6 +12,24 @@ from openpilot.starpilot.common.starpilot_variables import CITY_SPEED_LIMIT, MAX TRAFFIC_MODE_BP = [0., CITY_SPEED_LIMIT] PERSONALITY_BP = [45. * CV.MPH_TO_MS, 70. * CV.MPH_TO_MS] +LaneChangeState = log.LaneChangeState +LaneChangeDirection = log.LaneChangeDirection + +# Lane-change close-gap transition: while signalling out of the lane, temporarily +# hold a shorter follow distance so openpilot merges out smoothly and can accelerate +# through the maneuver instead of braking behind a lead it is about to leave. +# Ramps in gradually, snaps back to the normal gap when the safety gate trips. +LANE_CHANGE_MIN_T_FOLLOW = 0.5 # hard floor on the reduced gap (s) +LANE_CHANGE_GAP_RAMP_IN_RATE = 0.6 # seconds of headway per second, toward the shorter gap +LANE_CHANGE_GAP_RAMP_OUT_RATE = 4.0 # seconds of headway per second, back to the normal gap +LANE_CHANGE_ABORT_LEAD_BRAKE = 0.8 # lead decel that aborts the reduction (m/s^2) + +LANE_CHANGE_ACTIVE_STATES = ( + LaneChangeState.preLaneChange, + LaneChangeState.laneChangeStarting, + LaneChangeState.laneChangeFinishing, +) + def get_longitudinal_personality(sm): return sm["selfdriveState"].personality @@ -28,6 +48,9 @@ class StarPilotFollowing: self.speed_jerk = 0 self.t_follow = 0 + # Rate-limited lane-change follow distance (s). None until the first reduction. + self.lane_change_t_follow = None + def update(self, long_control_active, v_ego, sm, starpilot_toggles): personality = get_longitudinal_personality(sm) @@ -84,6 +107,8 @@ class StarPilotFollowing: if self.starpilot_planner.starpilot_weather.weather_id != 0: self.t_follow = min(self.t_follow + self.starpilot_planner.starpilot_weather.increase_following_distance, MAX_T_FOLLOW) + self.update_lane_change_gap(long_control_active, v_ego, sm, starpilot_toggles) + self.disable_throttle = False if self.starpilot_planner.tracking_lead and self.starpilot_planner.lead_one.status: lead_distance = self.starpilot_planner.lead_one.dRel @@ -98,6 +123,55 @@ class StarPilotFollowing: else: self.desired_follow_distance = 0 + def update_lane_change_gap(self, long_control_active, v_ego, sm, starpilot_toggles): + # Hold a shorter follow distance while signalling out of the lane so openpilot + # merges out smoothly and can accelerate through the maneuver, instead of braking + # behind a lead it is about to leave. The target is an absolute headway that can + # only ever shorten the current gap, never lengthen it. + if not long_control_active: + self.lane_change_t_follow = None + return + + target = self.t_follow + + if getattr(starpilot_toggles, "lane_change_close_gap", False): + meta = sm["modelV2"].meta + lane_change_active = meta.laneChangeState in LANE_CHANGE_ACTIVE_STATES + + if lane_change_active and self.lane_change_gap_safe(v_ego, sm, meta.laneChangeDirection, starpilot_toggles): + requested = float(np.clip(starpilot_toggles.lane_change_close_gap_seconds, LANE_CHANGE_MIN_T_FOLLOW, MAX_T_FOLLOW)) + # Absolute cap: only apply when it is actually shorter than the normal gap. + target = min(self.t_follow, requested) + + if self.lane_change_t_follow is None: + self.lane_change_t_follow = self.t_follow + + rate = LANE_CHANGE_GAP_RAMP_IN_RATE if target < self.lane_change_t_follow else LANE_CHANGE_GAP_RAMP_OUT_RATE + step = rate * DT_MDL + self.lane_change_t_follow = float(np.clip(target, + self.lane_change_t_follow - step, + self.lane_change_t_follow + step)) + + # Never let the ramp raise the gap above what the rest of the stack asked for. + self.t_follow = min(self.t_follow, max(self.lane_change_t_follow, LANE_CHANGE_MIN_T_FOLLOW)) + + def lane_change_gap_safe(self, v_ego, sm, direction, starpilot_toggles): + CS = sm["carState"] + if CS.standstill or v_ego < starpilot_toggles.minimum_lane_change_speed: + return False + + # Do not close the gap toward an occupied blindspot on the lane-change side. + if (direction == LaneChangeDirection.left and CS.leftBlindspot) or \ + (direction == LaneChangeDirection.right and CS.rightBlindspot): + return False + + lead = self.starpilot_planner.lead_one + if self.starpilot_planner.tracking_lead and lead.status: + if max(0.0, -float(lead.aLeadK)) >= LANE_CHANGE_ABORT_LEAD_BRAKE: + return False + + return True + def update_follow_values(self, lead_distance, v_ego, v_lead, starpilot_toggles): if starpilot_toggles.conditional_slower_lead and v_lead < v_ego: distance_factor = max(lead_distance - (v_lead * self.t_follow), 1) 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 935e45d33..0d5d435a6 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 @@ -228,6 +228,27 @@ "parent_key": "LaneChanges", "settings_tier": "simple" }, + { + "key": "LaneChangeCloseGap", + "label": "Close Gap On Lane Change", + "description": "Allows for a temporary shorter follow distance behind lead so that openpilot merges smoothly out of current lane, it will allow car to accelerate as it changes lanes.", + "data_type": "bool", + "ui_type": "toggle", + "parent_key": "LaneChanges", + "settings_tier": "simple" + }, + { + "key": "LaneChangeCloseGapSeconds", + "label": "Temporary Follow Distance", + "description": "Follow distance to hold while changing lanes. Only applied when shorter than your normal gap.", + "data_type": "float", + "ui_type": "numeric", + "min": 0.5, + "max": 3.0, + "step": 0.05, + "parent_key": "LaneChanges", + "settings_tier": "simple" + }, { "key": "LateralTune", "label": "Lateral Tuning",