Zikeji Tales

selfdrive/controls/lib/desire_helper.py

@@ -131,6 +131,22 @@ class DesireHelper:
       threshold *= NAV_KEEP_AMBIGUOUS_SPLIT_DISTANCE_SCALE
     return distance <= threshold
 
+  @staticmethod
+  def _nav_should_suppress_edge_lane_keep(nav_instruction_state):
+    maneuver_type = str(nav_instruction_state.get("maneuverType", ""))
+    if maneuver_type not in ("off ramp", "fork"):
+      return False
+
+    active_lane_direction = str(nav_instruction_state.get("activeLaneDirection", ""))
+    if active_lane_direction not in ("slightLeft", "left", "sharpLeft", "slightRight", "right", "sharpRight"):
+      return False
+
+    return (
+      int(nav_instruction_state.get("sameSideLaneCount", 0) or 0) > 1 and
+      bool(nav_instruction_state.get("activeLaneAtRoadEdge", False)) and
+      bool(nav_instruction_state.get("hasSharedSameSideLane", False))
+    )
+
   @staticmethod
   def _nav_effective_modifier(nav_instruction_state, carstate, maneuver_distance):
     modifier = str(nav_instruction_state.get("maneuverModifier", ""))
@@ -142,6 +158,9 @@ class DesireHelper:
       if not DesireHelper._nav_keep_is_imminent(carstate, maneuver_distance, maneuver_type, same_side_lane_count):
         return ""
 
+      if DesireHelper._nav_should_suppress_edge_lane_keep(nav_instruction_state):
+        return ""
+
       if active_lane_direction in ("slightLeft", "left"):
         return "slightLeft"
       if active_lane_direction in ("slightRight", "right"):

selfdrive/controls/tests/test_navigation_desires.py

@@ -165,6 +165,58 @@ def test_nav_desires_ambiguous_off_ramp_waits_longer_before_keep_right():
   assert helper.desire == log.Desire.none
 
 
+def test_nav_desires_edge_exit_lane_with_shared_transition_lane_does_not_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",
+    "sameSideLaneCount": 2,
+    "activeLaneAtRoadEdge": True,
+    "hasSharedSameSideLane": True,
+    "maneuverDistance": 10.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_shared_transition_lane_keeps_when_active_lane_is_not_outermost():
+  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",
+    "sameSideLaneCount": 2,
+    "activeLaneAtRoadEdge": False,
+    "hasSharedSameSideLane": True,
+    "maneuverDistance": 10.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_ambiguous_fork_slight_right_only_keeps_close_to_split():
   helper = DesireHelper()
   helper.nav_desires_allowed = True

starpilot/navigation/navigationd.py

@@ -275,14 +275,18 @@ class Navigationd:
       active_lane_side = "right"
 
     same_side_lane_count = 0
+    active_lane_at_road_edge = False
+    has_shared_same_side_lane = False
     if active_lane_side:
       same_side_directions = {"slightLeft", "left", "sharpLeft"} if active_lane_side == "left" else {"slightRight", "right", "sharpRight"}
+      active_lane_at_road_edge = active_lane_index == 0 if active_lane_side == "left" else active_lane_index == len(lanes) - 1
       for lane in lanes:
         if not isinstance(lane, dict):
           continue
         directions = {str(direction) for direction in lane.get("directions") or [] if direction}
         if directions & same_side_directions:
           same_side_lane_count += 1
+          has_shared_same_side_lane |= len(directions - same_side_directions) > 0
 
     state = {
       "valid": True,
@@ -290,6 +294,8 @@ class Navigationd:
       "maneuverType": str(payload.get("maneuverType") or ""),
       "activeLaneDirection": active_lane_direction,
       "activeLaneIndex": active_lane_index,
+      "activeLaneAtRoadEdge": active_lane_at_road_edge,
+      "hasSharedSameSideLane": has_shared_same_side_lane,
       "sameSideLaneCount": same_side_lane_count,
       "maneuverPrimaryText": str(payload.get("maneuverPrimaryText") or ""),
       "maneuverSecondaryText": str(payload.get("maneuverSecondaryText") or ""),