From 7051b79674913c7725f2f88c7035c2717ee56529 Mon Sep 17 00:00:00 2001 From: firestar5683 <168790843+firestar5683@users.noreply.github.com> Date: Fri, 29 May 2026 14:15:46 -0500 Subject: [PATCH] nav --- selfdrive/controls/lib/desire_helper.py | 30 ++++++++++-- .../controls/tests/test_navigation_desires.py | 46 +++++++++++++++++++ starpilot/navigation/navigationd.py | 11 +++++ 3 files changed, 84 insertions(+), 3 deletions(-) diff --git a/selfdrive/controls/lib/desire_helper.py b/selfdrive/controls/lib/desire_helper.py index 73c58bdac0..18625c972b 100644 --- a/selfdrive/controls/lib/desire_helper.py +++ b/selfdrive/controls/lib/desire_helper.py @@ -14,6 +14,8 @@ LANE_CHANGE_SPEED_MIN = 20 * CV.MPH_TO_MS LANE_CHANGE_TIME_MAX = 10. NAV_TURN_DISTANCE_SPEED_BREAKPOINTS = [0.0, 5.0, 10.0] NAV_TURN_DISTANCE_BREAKPOINTS = [20.0, 25.0, 30.0] +NAV_KEEP_DISTANCE_SPEED_BREAKPOINTS = [0.0, 15.0, 30.0] +NAV_KEEP_DISTANCE_BREAKPOINTS = [25.0, 90.0, 160.0] DESIRES = { LaneChangeDirection.none: { @@ -116,17 +118,39 @@ class DesireHelper: return distance <= float(np.interp(carstate.vEgo, NAV_TURN_DISTANCE_SPEED_BREAKPOINTS, NAV_TURN_DISTANCE_BREAKPOINTS)) + @staticmethod + def _nav_keep_is_imminent(carstate, maneuver_distance): + try: + distance = float(maneuver_distance) + except (TypeError, ValueError): + return False + + return distance <= float(np.interp(carstate.vEgo, NAV_KEEP_DISTANCE_SPEED_BREAKPOINTS, NAV_KEEP_DISTANCE_BREAKPOINTS)) + + @staticmethod + def _nav_effective_modifier(nav_instruction_state, carstate, maneuver_distance): + modifier = str(nav_instruction_state.get("maneuverModifier", "")) + maneuver_type = str(nav_instruction_state.get("maneuverType", "")) + active_lane_direction = str(nav_instruction_state.get("activeLaneDirection", "")) + + if modifier in ("left", "right") and maneuver_type in ("off ramp", "fork") and DesireHelper._nav_keep_is_imminent(carstate, maneuver_distance): + if active_lane_direction in ("slightLeft", "left"): + return "slightLeft" + if active_lane_direction in ("slightRight", "right"): + return "slightRight" + + return modifier + def _navigation_desire(self, carstate, lateral_active, starpilotPlan, starpilot_toggles): 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 - modifier = str(self._nav_instruction_state.get("maneuverModifier", "")) + maneuver_distance = self._nav_instruction_state.get("maneuverDistance", 0.0) + modifier = self._nav_effective_modifier(self._nav_instruction_state, carstate, maneuver_distance) if modifier == "": return log.Desire.none - maneuver_distance = self._nav_instruction_state.get("maneuverDistance", 0.0) - if modifier == "slightLeft": lane_change_direction = LaneChangeDirection.left desired_lane_width = starpilotPlan.laneWidthLeft diff --git a/selfdrive/controls/tests/test_navigation_desires.py b/selfdrive/controls/tests/test_navigation_desires.py index dcd75de7e7..f23a6c531f 100644 --- a/selfdrive/controls/tests/test_navigation_desires.py +++ b/selfdrive/controls/tests/test_navigation_desires.py @@ -93,6 +93,52 @@ def test_nav_desires_turn_right_waits_until_turn_is_close(): assert helper.desire == log.Desire.none +def test_nav_desires_off_ramp_lane_guidance_becomes_keep_right(): + helper = DesireHelper() + helper.nav_desires_allowed = True + helper._update_nav_params = lambda: None + helper._nav_instruction_state = { + "valid": True, + "maneuverType": "off ramp", + "maneuverModifier": "right", + "activeLaneDirection": "slightRight", + "maneuverDistance": 120.0, + } + + helper.update( + make_car_state(vEgo=22.5), + True, + 0.0, + make_plan(laneWidthRight=4.2), + make_toggles(nudgeless=True), + ) + + assert helper.desire == log.Desire.keepRight + + +def test_nav_desires_off_ramp_lane_guidance_waits_until_split_is_close(): + helper = DesireHelper() + helper.nav_desires_allowed = True + helper._update_nav_params = lambda: None + helper._nav_instruction_state = { + "valid": True, + "maneuverType": "off ramp", + "maneuverModifier": "right", + "activeLaneDirection": "slightRight", + "maneuverDistance": 300.0, + } + + helper.update( + make_car_state(vEgo=22.5), + True, + 0.0, + make_plan(laneWidthRight=4.2), + make_toggles(nudgeless=True), + ) + + assert helper.desire == log.Desire.none + + def test_nav_desires_do_not_override_lane_change_state_machine(): helper = DesireHelper() helper.nav_desires_allowed = True diff --git a/starpilot/navigation/navigationd.py b/starpilot/navigation/navigationd.py index 18c7a7ba2c..cf4813e070 100644 --- a/starpilot/navigation/navigationd.py +++ b/starpilot/navigation/navigationd.py @@ -254,11 +254,22 @@ class Navigationd: payload = route.build_instruction_payload(progress, use_vienna_sign=self.params.get_bool("UseVienna")) all_maneuvers = payload.get("allManeuvers") or [] next_maneuver = all_maneuvers[1] if len(all_maneuvers) > 1 and isinstance(all_maneuvers[1], dict) else {} + active_lane_direction = "" + for lane in payload.get("lanes") or []: + if not isinstance(lane, dict) or not bool(lane.get("active", False)): + continue + candidate = str(lane.get("activeDirection") or "") + if (not candidate or candidate == "none") and len(lane.get("directions") or []) == 1: + candidate = str((lane.get("directions") or [""])[0] or "") + if candidate and candidate != "none": + active_lane_direction = candidate + break state = { "valid": True, "maneuverModifier": str(payload.get("maneuverModifier") or ""), "maneuverType": str(payload.get("maneuverType") or ""), + "activeLaneDirection": active_lane_direction, "maneuverPrimaryText": str(payload.get("maneuverPrimaryText") or ""), "maneuverSecondaryText": str(payload.get("maneuverSecondaryText") or ""), "maneuverDistance": float(payload.get("maneuverDistance") or 0.0),