The Forked Man

selfdrive/controls/lib/desire_helper.py

@@ -138,16 +138,19 @@ class DesireHelper:
     active_lane_direction = str(nav_instruction_state.get("activeLaneDirection", ""))
     same_side_lane_count = int(nav_instruction_state.get("sameSideLaneCount", 0) or 0)
 
-    if maneuver_type in ("off ramp", "fork") and modifier in ("slightLeft", "slightRight"):
+    if maneuver_type in ("off ramp", "fork") and modifier in ("slightLeft", "left", "sharpLeft", "slightRight", "right", "sharpRight"):
       if not DesireHelper._nav_keep_is_imminent(carstate, maneuver_distance, maneuver_type, same_side_lane_count):
         return ""
 
-    if modifier in ("left", "right") and maneuver_type in ("off ramp", "fork") and DesireHelper._nav_keep_is_imminent(carstate, maneuver_distance, maneuver_type, same_side_lane_count):
       if active_lane_direction in ("slightLeft", "left"):
         return "slightLeft"
       if active_lane_direction in ("slightRight", "right"):
         return "slightRight"
 
+      # If lane guidance says the active lane stays straight, don't reinterpret the
+      # broader fork/off-ramp maneuver as a late turn into another branch.
+      return ""
+
     return modifier
 
   def _navigation_desire(self, carstate, lateral_active, starpilotPlan, starpilot_toggles):

selfdrive/controls/tests/test_navigation_desires.py

@@ -173,6 +173,7 @@ def test_nav_desires_ambiguous_fork_slight_right_only_keeps_close_to_split():
     "valid": True,
     "maneuverType": "fork",
     "maneuverModifier": "slightRight",
+    "activeLaneDirection": "slightRight",
     "sameSideLaneCount": 3,
     "maneuverDistance": 60.0,
   }
@@ -196,6 +197,7 @@ def test_nav_desires_ambiguous_fork_slight_right_does_not_nudge_too_early():
     "valid": True,
     "maneuverType": "fork",
     "maneuverModifier": "slightRight",
+    "activeLaneDirection": "slightRight",
     "sameSideLaneCount": 3,
     "maneuverDistance": 120.0,
   }
@@ -211,6 +213,29 @@ def test_nav_desires_ambiguous_fork_slight_right_does_not_nudge_too_early():
   assert helper.desire == log.Desire.none
 
 
+def test_nav_desires_fork_with_active_straight_lane_does_not_turn_left():
+  helper = DesireHelper()
+  helper.nav_desires_allowed = True
+  helper._update_nav_params = lambda: None
+  helper._nav_instruction_state = {
+    "valid": True,
+    "maneuverType": "fork",
+    "maneuverModifier": "left",
+    "activeLaneDirection": "straight",
+    "maneuverDistance": 15.0,
+  }
+
+  helper.update(
+    make_car_state(vEgo=5.0),
+    True,
+    0.0,
+    make_plan(laneWidthLeft=4.2),
+    make_toggles(nudgeless=True, minimum_lane_change_speed=10.0),
+  )
+
+  assert helper.desire == log.Desire.none
+
+
 def test_nav_desires_do_not_override_lane_change_state_machine():
   helper = DesireHelper()
   helper.nav_desires_allowed = True