nav

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

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

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),