From 93ec2a94e629eca4a263f6cd3ab3ed095f6403ca Mon Sep 17 00:00:00 2001 From: firestar5683 <168790843+firestar5683@users.noreply.github.com> Date: Mon, 15 Jun 2026 16:32:44 -0500 Subject: [PATCH] NudgeMe --- common/params_keys.h | 1 + selfdrive/controls/lib/desire_helper.py | 21 ++++-- .../controls/tests/test_navigation_desires.py | 66 +++++++++++++++++++ selfdrive/modeld/modeld.py | 2 +- selfdrive/modeld/modeld_v16.py | 2 +- starpilot/common/safe_mode.py | 1 + starpilot/common/starpilot_variables.py | 1 + .../tools/device_settings_layout.json | 8 +++ tools/StarPilot/feasibleparams.txt | 1 + 9 files changed, 95 insertions(+), 8 deletions(-) diff --git a/common/params_keys.h b/common/params_keys.h index 0ce9c5ba7..1ea83d10b 100644 --- a/common/params_keys.h +++ b/common/params_keys.h @@ -432,6 +432,7 @@ inline static std::unordered_map keys = { {"NoLogging", {PERSISTENT, BOOL, "0", "0", 2}}, {"NoUploads", {PERSISTENT, BOOL, "0", "0", 2}}, {"NudgelessLaneChange", {PERSISTENT, BOOL, "0", "0", 0}}, + {"NudgelessLaneChangeOnlyWhenEngaged", {PERSISTENT, BOOL, "0", "0", 1}}, {"NumericalTemp", {PERSISTENT, BOOL, "1", "0", 3}}, {"Offset1", {PERSISTENT, FLOAT, "5.0", "0.0", 0}}, {"Offset2", {PERSISTENT, FLOAT, "5.0", "0.0", 0}}, diff --git a/selfdrive/controls/lib/desire_helper.py b/selfdrive/controls/lib/desire_helper.py index 4e4f04c1e..d1bc71548 100644 --- a/selfdrive/controls/lib/desire_helper.py +++ b/selfdrive/controls/lib/desire_helper.py @@ -119,6 +119,13 @@ class DesireHelper: return distance <= float(np.interp(carstate.vEgo, NAV_TURN_DISTANCE_SPEED_BREAKPOINTS, NAV_TURN_DISTANCE_BREAKPOINTS)) + @staticmethod + def _nudgeless_enabled(starpilot_toggles, controls_enabled): + nudgeless = bool(getattr(starpilot_toggles, "nudgeless", False)) + if getattr(starpilot_toggles, "nudgeless_lane_change_only_when_engaged", False): + nudgeless &= bool(controls_enabled) + return nudgeless + @staticmethod def _nav_keep_is_imminent(carstate, maneuver_distance, maneuver_type="", same_side_lane_count=0): try: @@ -172,7 +179,7 @@ class DesireHelper: return modifier - def _navigation_desire(self, carstate, lateral_active, starpilotPlan, starpilot_toggles): + def _navigation_desire(self, carstate, lateral_active, starpilotPlan, starpilot_toggles, nudgeless_enabled): self._update_nav_params() if not self.nav_desires_allowed or not lateral_active or not bool(self._nav_instruction_state.get("valid", False)): return log.Desire.none @@ -185,14 +192,14 @@ class DesireHelper: if modifier == "slightLeft": lane_change_direction = LaneChangeDirection.left desired_lane_width = starpilotPlan.laneWidthLeft - nudgeless_allowed = starpilot_toggles.nudgeless and desired_lane_width >= starpilot_toggles.lane_detection_width + nudgeless_allowed = nudgeless_enabled and desired_lane_width >= starpilot_toggles.lane_detection_width if not carstate.rightBlinker and self._nav_keep_direction_is_clear(carstate, lane_change_direction): if self._nav_torque_applied(carstate, lane_change_direction) or nudgeless_allowed: return log.Desire.keepLeft elif modifier == "slightRight": lane_change_direction = LaneChangeDirection.right desired_lane_width = starpilotPlan.laneWidthRight - nudgeless_allowed = starpilot_toggles.nudgeless and desired_lane_width >= starpilot_toggles.lane_detection_width + nudgeless_allowed = nudgeless_enabled and desired_lane_width >= starpilot_toggles.lane_detection_width if not carstate.leftBlinker and self._nav_keep_direction_is_clear(carstate, lane_change_direction): if self._nav_torque_applied(carstate, lane_change_direction) or nudgeless_allowed: return log.Desire.keepRight @@ -209,11 +216,13 @@ class DesireHelper: def get_lane_change_direction(CS): return LaneChangeDirection.left if CS.leftBlinker else LaneChangeDirection.right - def update(self, carstate, lateral_active, lane_change_prob, starpilotPlan, starpilot_toggles): + def update(self, carstate, lateral_active, lane_change_prob, starpilotPlan, starpilot_toggles, controls_enabled=None): v_ego = carstate.vEgo one_blinker = carstate.leftBlinker != carstate.rightBlinker below_lane_change_speed = v_ego < starpilot_toggles.minimum_lane_change_speed cruise_state = getattr(carstate, "cruiseState", None) + controls_enabled = bool(getattr(cruise_state, "enabled", False)) if controls_enabled is None else bool(controls_enabled) + nudgeless_enabled = self._nudgeless_enabled(starpilot_toggles, controls_enabled) lane_changes_allowed = starpilot_toggles.lane_changes lane_changes_allowed &= not getattr(starpilot_toggles, "lane_changes_require_cruise", False) or bool(getattr(cruise_state, "enabled", False)) @@ -244,7 +253,7 @@ class DesireHelper: if torque_applied: self.lane_change_wait_timer = starpilot_toggles.lane_change_delay else: - torque_applied |= starpilot_toggles.nudgeless + torque_applied |= nudgeless_enabled torque_applied &= self.lane_change_wait_timer >= starpilot_toggles.lane_change_delay desired_lane_width = starpilotPlan.laneWidthLeft if self.lane_change_direction == LaneChangeDirection.left else starpilotPlan.laneWidthRight @@ -312,6 +321,6 @@ class DesireHelper: self.lane_change_wait_timer = 0.0 - nav_desire = self._navigation_desire(carstate, lateral_active, starpilotPlan, starpilot_toggles) + nav_desire = self._navigation_desire(carstate, lateral_active, starpilotPlan, starpilot_toggles, nudgeless_enabled) if nav_desire != log.Desire.none and self.lane_change_state == LaneChangeState.off: self.desire = nav_desire diff --git a/selfdrive/controls/tests/test_navigation_desires.py b/selfdrive/controls/tests/test_navigation_desires.py index 9b56249c4..b9cd35420 100644 --- a/selfdrive/controls/tests/test_navigation_desires.py +++ b/selfdrive/controls/tests/test_navigation_desires.py @@ -28,6 +28,7 @@ def make_toggles(**overrides): "lane_detection_width": 3.0, "minimum_lane_change_speed": 10.0, "nudgeless": True, + "nudgeless_lane_change_only_when_engaged": False, "one_lane_change": False, "use_turn_desires": False, "lane_changes_require_cruise": False, @@ -354,6 +355,71 @@ def test_lane_changes_without_cruise_requirement_keep_existing_behavior(): assert helper.lane_change_direction == LaneChangeDirection.left +def test_nudgeless_only_when_engaged_allows_automatic_lane_change_when_engaged(): + helper = DesireHelper() + + for _ in range(2): + helper.update( + make_car_state(leftBlinker=True), + True, + 0.0, + make_plan(), + make_toggles(nudgeless_lane_change_only_when_engaged=True), + controls_enabled=True, + ) + + assert helper.lane_change_state == LaneChangeState.laneChangeStarting + assert helper.lane_change_direction == LaneChangeDirection.left + + +def test_nudgeless_only_when_engaged_requires_nudge_when_aol_only(): + helper = DesireHelper() + toggles = make_toggles(nudgeless_lane_change_only_when_engaged=True) + + for _ in range(2): + helper.update( + make_car_state(leftBlinker=True), + True, + 0.0, + make_plan(), + toggles, + controls_enabled=False, + ) + + assert helper.lane_change_state == LaneChangeState.preLaneChange + assert helper.lane_change_direction == LaneChangeDirection.left + + helper.update( + make_car_state(leftBlinker=True, steeringPressed=True, steeringTorque=1.0), + True, + 0.0, + make_plan(), + toggles, + controls_enabled=False, + ) + + assert helper.lane_change_state == LaneChangeState.laneChangeStarting + assert helper.lane_change_direction == LaneChangeDirection.left + + +def test_nav_desires_nudgeless_only_when_engaged_blocks_keep_when_aol_only(): + helper = DesireHelper() + helper.nav_desires_allowed = True + helper._update_nav_params = lambda: None + helper._nav_instruction_state = {"valid": True, "maneuverModifier": "slightLeft"} + + helper.update( + make_car_state(vEgo=20.0), + True, + 0.0, + make_plan(laneWidthLeft=4.2), + make_toggles(nudgeless=True, nudgeless_lane_change_only_when_engaged=True), + controls_enabled=False, + ) + + assert helper.desire == log.Desire.none + + def test_nav_desires_disabled_leave_desire_unchanged(): helper = DesireHelper() helper.nav_desires_allowed = False diff --git a/selfdrive/modeld/modeld.py b/selfdrive/modeld/modeld.py index 46592b322..2efbabeb3 100755 --- a/selfdrive/modeld/modeld.py +++ b/selfdrive/modeld/modeld.py @@ -699,7 +699,7 @@ def main(demo=False): l_lane_change_prob = desire_state[log.Desire.laneChangeLeft] r_lane_change_prob = desire_state[log.Desire.laneChangeRight] lane_change_prob = l_lane_change_prob + r_lane_change_prob - DH.update(sm['carState'], sm['carControl'].latActive, lane_change_prob, sm['starpilotPlan'], starpilot_toggles) + DH.update(sm['carState'], sm['carControl'].latActive, lane_change_prob, sm['starpilotPlan'], starpilot_toggles, sm['carControl'].enabled) modelv2_send.modelV2.meta.laneChangeState = DH.lane_change_state modelv2_send.modelV2.meta.laneChangeDirection = DH.lane_change_direction starpilot_modelv2_send.starpilotModelV2.turnDirection = DH.turn_direction diff --git a/selfdrive/modeld/modeld_v16.py b/selfdrive/modeld/modeld_v16.py index b2a50a90e..1146c56c3 100644 --- a/selfdrive/modeld/modeld_v16.py +++ b/selfdrive/modeld/modeld_v16.py @@ -479,7 +479,7 @@ def main(demo=False): l_lane_change_prob = desire_state[log.Desire.laneChangeLeft] r_lane_change_prob = desire_state[log.Desire.laneChangeRight] lane_change_prob = l_lane_change_prob + r_lane_change_prob - desire_helper.update(sm["carState"], sm["carControl"].latActive, lane_change_prob, sm["starpilotPlan"], starpilot_toggles) + desire_helper.update(sm["carState"], sm["carControl"].latActive, lane_change_prob, sm["starpilotPlan"], starpilot_toggles, sm["carControl"].enabled) modelv2_send.modelV2.meta.laneChangeState = desire_helper.lane_change_state modelv2_send.modelV2.meta.laneChangeDirection = desire_helper.lane_change_direction starpilot_modelv2_send.starpilotModelV2.turnDirection = desire_helper.turn_direction diff --git a/starpilot/common/safe_mode.py b/starpilot/common/safe_mode.py index afbfe3a00..5aa0f8bfb 100644 --- a/starpilot/common/safe_mode.py +++ b/starpilot/common/safe_mode.py @@ -37,6 +37,7 @@ SAFE_MODE_MANAGED_KEYS = ( "LaneDetectionWidth", "MinimumLaneChangeSpeed", "NudgelessLaneChange", + "NudgelessLaneChangeOnlyWhenEngaged", "OneLaneChange", "NNFF", "NNFFLite", diff --git a/starpilot/common/starpilot_variables.py b/starpilot/common/starpilot_variables.py index 4d72cb2d8..b5bdd133a 100644 --- a/starpilot/common/starpilot_variables.py +++ b/starpilot/common/starpilot_variables.py @@ -967,6 +967,7 @@ class StarPilotVariables: 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.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) # Lane change pace: 1 = smoothest (~8 s target), 10 = stock (no clamp applied) diff --git a/starpilot/system/the_pond/assets/components/tools/device_settings_layout.json b/starpilot/system/the_pond/assets/components/tools/device_settings_layout.json index 565aad390..e2650a50d 100644 --- a/starpilot/system/the_pond/assets/components/tools/device_settings_layout.json +++ b/starpilot/system/the_pond/assets/components/tools/device_settings_layout.json @@ -137,6 +137,14 @@ "ui_type": "toggle", "parent_key": "LaneChanges" }, + { + "key": "NudgelessLaneChangeOnlyWhenEngaged", + "label": "Automatic Lane Changes Only When Engaged", + "description": "Require a steering-wheel nudge for lane changes while using Always On Lateral without full engagement.", + "data_type": "bool", + "ui_type": "toggle", + "parent_key": "LaneChanges" + }, { "key": "LaneChangeTime", "label": "Lane Change Delay", diff --git a/tools/StarPilot/feasibleparams.txt b/tools/StarPilot/feasibleparams.txt index 2c3861042..340381d69 100644 --- a/tools/StarPilot/feasibleparams.txt +++ b/tools/StarPilot/feasibleparams.txt @@ -203,6 +203,7 @@ NoLogging NoUploads NostalgiaMode NudgelessLaneChange +NudgelessLaneChangeOnlyWhenEngaged NumericalTemp Offroad_ExcessiveActuation Offset1