diff --git a/selfdrive/controls/lib/desire_helper.py b/selfdrive/controls/lib/desire_helper.py index 18625c972b..634ad2e1fd 100644 --- a/selfdrive/controls/lib/desire_helper.py +++ b/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"): diff --git a/selfdrive/controls/tests/test_navigation_desires.py b/selfdrive/controls/tests/test_navigation_desires.py index f23a6c531f..1b1ce851ff 100644 --- a/selfdrive/controls/tests/test_navigation_desires.py +++ b/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 diff --git a/starpilot/navigation/navigationd.py b/starpilot/navigation/navigationd.py index cf4813e070..2cdcf1c1a6 100644 --- a/starpilot/navigation/navigationd.py +++ b/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),