diff --git a/selfdrive/controls/lib/desire_helper.py b/selfdrive/controls/lib/desire_helper.py index 721f20f120..4e4f04c1e8 100644 --- a/selfdrive/controls/lib/desire_helper.py +++ b/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"): diff --git a/selfdrive/controls/tests/test_navigation_desires.py b/selfdrive/controls/tests/test_navigation_desires.py index 85042c759f..9b56249c41 100644 --- a/selfdrive/controls/tests/test_navigation_desires.py +++ b/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 diff --git a/starpilot/navigation/navigationd.py b/starpilot/navigation/navigationd.py index 2cdcf1c1a6..a8f0a710b3 100644 --- a/starpilot/navigation/navigationd.py +++ b/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 ""),