Navigation: Breaking Dawn

selfdrive/controls/lib/desire_helper.py

@@ -16,6 +16,7 @@ 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]
+NAV_KEEP_AMBIGUOUS_SPLIT_DISTANCE_SCALE = 0.6
 
 DESIRES = {
   LaneChangeDirection.none: {
@@ -119,21 +120,29 @@ 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):
+  def _nav_keep_is_imminent(carstate, maneuver_distance, maneuver_type="", same_side_lane_count=0):
     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))
+    threshold = float(np.interp(carstate.vEgo, NAV_KEEP_DISTANCE_SPEED_BREAKPOINTS, NAV_KEEP_DISTANCE_BREAKPOINTS))
+    if maneuver_type in ("off ramp", "fork") and int(same_side_lane_count or 0) > 1:
+      threshold *= NAV_KEEP_AMBIGUOUS_SPLIT_DISTANCE_SCALE
+    return distance <= threshold
 
   @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", ""))
+    same_side_lane_count = int(nav_instruction_state.get("sameSideLaneCount", 0) or 0)
 
-    if modifier in ("left", "right") and maneuver_type in ("off ramp", "fork") and DesireHelper._nav_keep_is_imminent(carstate, maneuver_distance):
+    if maneuver_type in ("off ramp", "fork") and modifier in ("slightLeft", "slightRight"):
+      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"):

selfdrive/controls/tests/test_navigation_desires.py

@@ -139,6 +139,76 @@ def test_nav_desires_off_ramp_lane_guidance_waits_until_split_is_close():
   assert helper.desire == log.Desire.none
 
 
+def test_nav_desires_ambiguous_off_ramp_waits_longer_before_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": 3,
+    "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.none
+
+
+def test_nav_desires_ambiguous_fork_slight_right_only_keeps_close_to_split():
+  helper = DesireHelper()
+  helper.nav_desires_allowed = True
+  helper._update_nav_params = lambda: None
+  helper._nav_instruction_state = {
+    "valid": True,
+    "maneuverType": "fork",
+    "maneuverModifier": "slightRight",
+    "sameSideLaneCount": 3,
+    "maneuverDistance": 60.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_does_not_nudge_too_early():
+  helper = DesireHelper()
+  helper.nav_desires_allowed = True
+  helper._update_nav_params = lambda: None
+  helper._nav_instruction_state = {
+    "valid": True,
+    "maneuverType": "fork",
+    "maneuverModifier": "slightRight",
+    "sameSideLaneCount": 3,
+    "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.none
+
+
 def test_nav_desires_do_not_override_lane_change_state_machine():
   helper = DesireHelper()
   helper.nav_desires_allowed = True

starpilot/navigation/navigationd.py

@@ -255,7 +255,9 @@ class Navigationd:
     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 []:
+    active_lane_index = -1
+    lanes = payload.get("lanes") or []
+    for index, lane in enumerate(lanes):
       if not isinstance(lane, dict) or not bool(lane.get("active", False)):
         continue
       candidate = str(lane.get("activeDirection") or "")
@@ -263,13 +265,32 @@ class Navigationd:
         candidate = str((lane.get("directions") or [""])[0] or "")
       if candidate and candidate != "none":
         active_lane_direction = candidate
+        active_lane_index = index
         break
 
+    active_lane_side = ""
+    if active_lane_direction in ("slightLeft", "left", "sharpLeft"):
+      active_lane_side = "left"
+    elif active_lane_direction in ("slightRight", "right", "sharpRight"):
+      active_lane_side = "right"
+
+    same_side_lane_count = 0
+    if active_lane_side:
+      same_side_directions = {"slightLeft", "left", "sharpLeft"} if active_lane_side == "left" else {"slightRight", "right", "sharpRight"}
+      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
+
     state = {
       "valid": True,
       "maneuverModifier": str(payload.get("maneuverModifier") or ""),
       "maneuverType": str(payload.get("maneuverType") or ""),
       "activeLaneDirection": active_lane_direction,
+      "activeLaneIndex": active_lane_index,
+      "sameSideLaneCount": same_side_lane_count,
       "maneuverPrimaryText": str(payload.get("maneuverPrimaryText") or ""),
       "maneuverSecondaryText": str(payload.get("maneuverSecondaryText") or ""),
       "maneuverDistance": float(payload.get("maneuverDistance") or 0.0),