diff --git a/scripts/speed_limit_vision/build_review_classifier_dataset.py b/scripts/speed_limit_vision/build_review_classifier_dataset.py index 4158579e9..8d48fc18e 100644 --- a/scripts/speed_limit_vision/build_review_classifier_dataset.py +++ b/scripts/speed_limit_vision/build_review_classifier_dataset.py @@ -20,6 +20,19 @@ def parse_args() -> argparse.Namespace: parser.add_argument("--output", type=Path, required=True, help="New isolated dataset root.") parser.add_argument("--positive-manifest", type=Path, action="append", default=[], help="Reviewed positive crop manifest. Repeat as needed.") parser.add_argument("--reject-manifest", type=Path, action="append", default=[], help="Reviewed classifier reject manifest. Repeat as needed.") + parser.add_argument( + "--exclude-base-record-key", + action="append", + default=[], + help="Remove inherited samples whose staged filename contains this corrected record key. Repeat as needed.", + ) + parser.add_argument( + "--repeat-reject-record", + action="append", + default=[], + metavar="RECORD_KEY=COUNT", + help="Stage a reviewed reject COUNT times to give a corrected hard negative more training weight.", + ) parser.add_argument( "--advisory-as-reject", action="store_true", @@ -76,12 +89,45 @@ def keep_advisory_reject(row: dict[str, str], fraction: float) -> bool: return int.from_bytes(digest[:8], "big") / 2**64 < fraction +def safe_record_key(record_key: str) -> str: + return "".join(char if char.isalnum() or char in "._-" else "_" for char in record_key)[:100] + + +def remove_inherited_records(root: Path, record_keys: list[str]) -> int: + safe_keys = tuple(filter(None, (safe_record_key(record_key) for record_key in record_keys))) + if not safe_keys: + return 0 + removed = 0 + for split in ("train", "val"): + for path in (root / split).rglob("*"): + if path.is_file() and any(record_key in path.name for record_key in safe_keys): + path.unlink() + removed += 1 + return removed + + +def parse_reject_repeat_counts(specs: list[str]) -> dict[str, int]: + repeat_counts: dict[str, int] = {} + for spec in specs: + record_key, separator, count_text = spec.rpartition("=") + if not separator or not record_key: + raise ValueError(f"Invalid --repeat-reject-record value: {spec!r}") + try: + count = int(count_text) + except ValueError as exc: + raise ValueError(f"Invalid reject repeat count: {spec!r}") from exc + if count < 1: + raise ValueError(f"Reject repeat count must be at least 1: {spec!r}") + repeat_counts[record_key] = count + return repeat_counts + + def stage_crop(source: Path, destination_dir: Path, record_key: str) -> bool: if not source.is_file(): return False digest = hashlib.sha256(source.read_bytes()).hexdigest()[:16] suffix = source.suffix.lower() if source.suffix.lower() in (".jpg", ".jpeg", ".png") else ".jpg" - safe_key = "".join(char if char.isalnum() or char in "._-" else "_" for char in record_key)[:100] + safe_key = safe_record_key(record_key) destination_dir.mkdir(parents=True, exist_ok=True) destination = destination_dir / f"review_{safe_key}_{digest}{suffix}" if not destination.exists(): @@ -103,6 +149,8 @@ def main() -> int: raise FileExistsError(f"Output dataset already exists: {output}") shutil.copytree(base, output, copy_function=shutil.copyfile) appledouble_removed = remove_appledouble_files(output) + inherited_records_removed = remove_inherited_records(output, args.exclude_base_record_key) + reject_repeat_counts = parse_reject_repeat_counts(args.repeat_reject_record) positive_counts: Counter[str] = Counter() reject_counts: Counter[str] = Counter() @@ -132,10 +180,16 @@ def main() -> int: for row in read_rows(args.reject_manifest): split = row.get("split", "") source = Path(row.get("crop_path", "")).expanduser() - if split not in ("train", "val") or not stage_crop(source, output / split / "reject", row.get("record_key", "reject")): + record_key = row.get("record_key", "reject") + repeat_count = reject_repeat_counts.get(record_key, 1) if split == "train" else 1 + staged = split in ("train", "val") + for repeat_index in range(repeat_count): + staged_key = record_key if repeat_index == 0 else f"{record_key}_repeat_{repeat_index:03d}" + staged = staged and stage_crop(source, output / split / "reject", staged_key) + if not staged: skipped += 1 continue - reject_counts[split] += 1 + reject_counts[split] += repeat_count appledouble_removed += remove_appledouble_files(output) for split in ("train", "val"): @@ -150,6 +204,7 @@ def main() -> int: "reject_counts": dict(sorted(reject_counts.items())), "skipped": skipped, "appledouble_removed": appledouble_removed, + "inherited_records_removed": inherited_records_removed, } summary_path = output / "review_dataset_summary.json" summary_path.write_text(json.dumps(summary, indent=2, sort_keys=True) + "\n", encoding="utf-8") diff --git a/scripts/speed_limit_vision/import_manual_review_queue.py b/scripts/speed_limit_vision/import_manual_review_queue.py index 7edaf904b..a7d7002a5 100644 --- a/scripts/speed_limit_vision/import_manual_review_queue.py +++ b/scripts/speed_limit_vision/import_manual_review_queue.py @@ -305,6 +305,58 @@ def classifier_reject_row(row: dict[str, str], split: str) -> dict[str, object]: } +RUNTIME_REJECT_CROP_EXPANSIONS = ( + (0.00, 0.00, 0.00, 0.00), + (0.10, 0.06, 0.10, 0.12), + (0.00, 0.00, 0.18, 0.18), +) + + +def classifier_reject_variant_rows( + row: dict[str, str], + split: str, + output_dir: Path, + overwrite: bool, +) -> list[dict[str, object]]: + rows = [classifier_reject_row(row, split)] + if row.get("review_ignore_reason") != "conditional_restriction": + return rows + + frame_path = Path(row.get("frame_path", "")).expanduser() + frame = cv2.imread(str(frame_path)) + bbox = parse_bbox(row.get("review_bbox") or row.get("bbox", "")) + if frame is None or bbox is None: + raise RuntimeError(f"Cannot generate conditional reject crops for {row['record_key']}: unreadable frame or bbox") + + image_h, image_w = frame.shape[:2] + x1, y1, x2, y2 = bbox + box_width = x2 - x1 + box_height = y2 - y1 + reject_dir = output_dir / "corrected_classifier_reject_crops" + ensure_dir(reject_dir) + for index, (expand_left, expand_top, expand_right, expand_bottom) in enumerate(RUNTIME_REJECT_CROP_EXPANSIONS): + crop_bbox = ( + max(int(x1 - box_width * expand_left), 0), + max(int(y1 - box_height * expand_top), 0), + min(int(x2 + box_width * expand_right), image_w), + min(int(y2 + box_height * expand_bottom), image_h), + ) + crop_x1, crop_y1, crop_x2, crop_y2 = crop_bbox + crop = frame[crop_y1:crop_y2, crop_x1:crop_x2] + crop_path = reject_dir / f"{safe_stem(row['record_key'])}_runtime_expansion_{index}.jpg" + if crop.size == 0: + raise RuntimeError(f"Cannot generate conditional reject crop for {row['record_key']}: empty bbox {crop_bbox}") + if overwrite or not crop_path.is_file(): + if not cv2.imwrite(str(crop_path), crop, [cv2.IMWRITE_JPEG_QUALITY, 94]): + raise RuntimeError(f"Cannot write conditional reject crop for {row['record_key']}: {crop_path}") + variant = classifier_reject_row(row, split) + variant["record_key"] = f"{row['record_key']}_runtime_expansion_{index}" + variant["crop_path"] = str(crop_path) + variant["crop_bbox"] = ",".join(str(value) for value in crop_bbox) + rows.append(variant) + return rows + + def corrected_classifier_crop( row: dict[str, str], output_dir: Path, @@ -508,7 +560,7 @@ def main() -> int: for row in classifier_reject_rows: split = split_for_key(split_group_key(row), args.val_modulo, args.val_remainder) - reject_rows.append(classifier_reject_row(row, split)) + reject_rows.extend(classifier_reject_variant_rows(row, split, output_dir, args.overwrite)) write_csv(classifier_manifest, CLASSIFIER_FIELDNAMES, classifier_rows) write_csv(runtime_manifest, RUNTIME_FIELDNAMES, runtime_rows) diff --git a/scripts/speed_limit_vision/test_review_pipeline.py b/scripts/speed_limit_vision/test_review_pipeline.py index d310ba7ed..a16d22ebc 100644 --- a/scripts/speed_limit_vision/test_review_pipeline.py +++ b/scripts/speed_limit_vision/test_review_pipeline.py @@ -235,3 +235,53 @@ def test_corrected_bbox_requires_readable_source_frame(tmp_path): with pytest.raises(RuntimeError, match="unreadable frame"): import_queue.corrected_classifier_crop(row, tmp_path, overwrite=False) + + +def test_corrected_record_removes_inherited_classifier_sample(tmp_path): + stale = tmp_path / "train" / "55" / "base_review_bad_record_key_hash.jpg" + retained = tmp_path / "train" / "55" / "base_review_other_record_hash.jpg" + stale.parent.mkdir(parents=True) + stale.write_bytes(b"stale") + retained.write_bytes(b"retained") + + removed = build_review_classifier.remove_inherited_records(tmp_path, ["bad:record/key"]) + + assert removed == 1 + assert not stale.exists() + assert retained.exists() + + +def test_reject_repeat_spec_preserves_record_key_punctuation(): + counts = build_review_classifier.parse_reject_repeat_counts(["route/sign=track:55=32"]) + + assert counts == {"route/sign=track:55": 32} + + with pytest.raises(ValueError, match="at least 1"): + build_review_classifier.parse_reject_repeat_counts(["bad-record=0"]) + + +def test_conditional_reject_generates_runtime_crop_expansions(tmp_path): + import cv2 + import numpy as np + + frame_path = tmp_path / "frame.jpg" + crop_path = tmp_path / "crop.jpg" + frame = np.zeros((100, 200, 3), dtype=np.uint8) + cv2.imwrite(str(frame_path), frame) + cv2.imwrite(str(crop_path), frame[20:80, 60:140]) + row = { + "record_key": "conditional-sign", + "frame_path": str(frame_path), + "crop_path": str(crop_path), + "bbox": "60,20,140,80", + "review_bbox": "60,20,140,80", + "review_ignore_reason": "conditional_restriction", + } + + rows = import_queue.classifier_reject_variant_rows(row, "train", tmp_path, overwrite=False) + + assert len(rows) == 4 + assert rows[1]["crop_bbox"] == "60,20,140,80" + assert rows[2]["crop_bbox"] == "52,16,148,87" + assert rows[3]["crop_bbox"] == "60,20,154,90" + assert all(Path(variant["crop_path"]).is_file() for variant in rows) diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index 4f9defce0..12fa3a8e0 100755 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -89,6 +89,8 @@ STABLE_FOLLOW_CRUISE_PULLAWAY_MAX_REL_SPEED = 2.0 STABLE_FOLLOW_CRUISE_PULLAWAY_MAX_HEADWAY_MARGIN = 0.35 STABLE_FOLLOW_CRUISE_PULLAWAY_MIN_HEADWAY_MARGIN = -0.10 STABLE_FOLLOW_CRUISE_PULLAWAY_HYSTERESIS_MAX = 1.75 +VISION_FOLLOW_CRUISE_HOLD_MIN_MODEL_PROB = 0.95 +VISION_FOLLOW_CRUISE_HOLD_MAX_CRUISE_ADVANTAGE = 2.0 NEAR_DUPLICATE_LEAD_SOURCE_MIN_SPEED = 20.0 NEAR_DUPLICATE_IDENTICAL_RADAR_SOURCE_MIN_SPEED = 10.0 NEAR_DUPLICATE_LEAD_SOURCE_MIN_MODEL_PROB = 0.9 @@ -747,6 +749,27 @@ class LongitudinalMpc: return prev_source + def get_vision_follow_cruise_hold(self, prev_source, lead_one, lead_two, + lead_0_obstacle, lead_1_obstacle, cruise_obstacle, + v_ego, t_follow, tracking_lead): + if not tracking_lead or prev_source not in ("lead0", "lead1"): + return None + + prev_lead = lead_one if prev_source == "lead0" else lead_two + if prev_lead is None or not prev_lead.status or bool(getattr(prev_lead, "radar", False)): + return None + if float(getattr(prev_lead, "modelProb", 0.0)) < VISION_FOLLOW_CRUISE_HOLD_MIN_MODEL_PROB: + return None + if self.get_stable_follow_cruise_hysteresis(prev_lead, v_ego, t_follow) <= 0.0: + return None + + prev_lead_obstacle = float(lead_0_obstacle if prev_source == "lead0" else lead_1_obstacle) + cruise_advantage = prev_lead_obstacle - float(cruise_obstacle) + if cruise_advantage > VISION_FOLLOW_CRUISE_HOLD_MAX_CRUISE_ADVANTAGE: + return None + + return prev_source + def get_identical_radar_duplicate_cruise_bias(self, lead_one, lead_two, v_ego, t_follow): if float(v_ego) < IDENTICAL_RADAR_DUPLICATE_CRUISE_BIAS_MIN_SPEED: return 0.0 @@ -862,6 +885,18 @@ class LongitudinalMpc: v_ego, t_follow, ) + if sticky_source is None: + sticky_source = self.get_vision_follow_cruise_hold( + prev_source, + lead_one, + lead_two, + lead_0_obstacle[0], + lead_1_obstacle[0], + cruise_obstacle[0], + v_ego, + t_follow, + tracking_lead, + ) self.source = sticky_source or candidate_source # These are not used in ACC mode diff --git a/selfdrive/controls/tests/test_conditional_experimental_mode.py b/selfdrive/controls/tests/test_conditional_experimental_mode.py index ece5510ab..7df4826e8 100644 --- a/selfdrive/controls/tests/test_conditional_experimental_mode.py +++ b/selfdrive/controls/tests/test_conditional_experimental_mode.py @@ -504,6 +504,87 @@ def test_post_stop_slow_lead_trigger_is_suppressed_after_red_light_release(monke assert cem.status_value == conditional_experimental_mode_module.CEStatus["LEAD"] +def test_slow_lead_mode_release_waits_for_stable_credible_lead(monkeypatch): + v_ego = 20.0 + cem = make_cem( + model_length=100.0, + tracking_lead=True, + lead_status=True, + lead_d_rel=34.0, + lead_v_lead=18.5, + lead_model_prob=0.99, + ) + toggles = make_update_toggles() + toggles.conditional_lead = True + toggles.conditional_slower_lead = True + sm = make_update_sm(standstill=False) + slow_lead_active = [True] + now = [100.0] + + monkeypatch.setattr(conditional_experimental_mode_module.time, "monotonic", lambda: now[0]) + + def update_conditions(*args, **kwargs): + cem.slow_lead_detected = slow_lead_active[0] + + monkeypatch.setattr(cem, "update_conditions", update_conditions) + + cem.update(v_ego, sm, toggles) + assert cem.experimental_mode + assert cem.status_value == conditional_experimental_mode_module.CEStatus["LEAD"] + + slow_lead_active[0] = False + now[0] = 100.9 + cem.update(v_ego, sm, toggles) + assert cem.experimental_mode + assert cem.status_value == conditional_experimental_mode_module.CEStatus["LEAD"] + + now[0] = 101.6 + cem.update(v_ego, sm, toggles) + assert not cem.experimental_mode + assert cem.params_memory.get_int("CEStatus") == conditional_experimental_mode_module.CEStatus["OFF"] + + +def test_slow_lead_mode_release_does_not_hold_missing_lead(monkeypatch): + v_ego = 20.0 + cem = make_cem( + model_length=100.0, + tracking_lead=True, + lead_status=True, + lead_d_rel=34.0, + lead_v_lead=18.5, + lead_model_prob=0.99, + ) + toggles = make_update_toggles() + toggles.conditional_lead = True + toggles.conditional_slower_lead = True + sm = make_update_sm(standstill=False) + slow_lead_active = [True] + now = [200.0] + + monkeypatch.setattr(conditional_experimental_mode_module.time, "monotonic", lambda: now[0]) + + def update_conditions(*args, **kwargs): + cem.slow_lead_detected = slow_lead_active[0] + + monkeypatch.setattr(cem, "update_conditions", update_conditions) + + cem.update(v_ego, sm, toggles) + assert cem.experimental_mode + + slow_lead_active[0] = False + cem.starpilot_planner.lead_one.status = False + cem.starpilot_planner.tracking_lead = False + now[0] = 200.6 + cem.update(v_ego, sm, toggles) + assert cem.experimental_mode + + now[0] = 200.9 + cem.update(v_ego, sm, toggles) + + assert not cem.experimental_mode + assert cem.params_memory.get_int("CEStatus") == conditional_experimental_mode_module.CEStatus["OFF"] + + def test_standstill_update_can_activate_exp_from_dashboard_stop_sign(monkeypatch): cem = make_cem(model_length=80.0, model_stopped=False) toggles = make_update_toggles() diff --git a/selfdrive/controls/tests/test_longitudinal_planner.py b/selfdrive/controls/tests/test_longitudinal_planner.py index 74a6527e7..67fa55f6a 100644 --- a/selfdrive/controls/tests/test_longitudinal_planner.py +++ b/selfdrive/controls/tests/test_longitudinal_planner.py @@ -3092,6 +3092,87 @@ def test_stable_follow_cruise_hysteresis_skips_fast_closing_radar_lead(): assert hysteresis == 0.0 +def test_vision_follow_cruise_hold_keeps_high_confidence_matched_lead_through_small_crossover(): + v_ego = 22.5 + t_follow = 1.20 + CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) + planner = LongitudinalPlanner(CP, init_v=v_ego) + lead_one = make_lead(status=True, d_rel=32.0, v_lead=22.0, a_lead=-0.02, radar=False, model_prob=0.99) + lead_two = make_lead(status=False) + + sticky = planner.mpc.get_vision_follow_cruise_hold( + "lead0", + lead_one, + lead_two, + 101.0, + 200.0, + 100.0, + v_ego, + t_follow, + True, + ) + + assert sticky == "lead0" + + +@pytest.mark.parametrize("tracking_lead, radar, model_prob, cruise_obstacle", [ + (False, False, 0.99, 100.0), + (True, True, 1.0, 100.0), + (True, False, 0.80, 100.0), + (True, False, 0.99, 98.0), +]) +def test_vision_follow_cruise_hold_skips_nonmatching_or_clear_cruise_cases( + tracking_lead, radar, model_prob, cruise_obstacle, +): + v_ego = 22.5 + t_follow = 1.20 + CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) + planner = LongitudinalPlanner(CP, init_v=v_ego) + lead_one = make_lead(status=True, d_rel=32.0, v_lead=22.0, a_lead=-0.02, radar=radar, model_prob=model_prob) + lead_two = make_lead(status=False) + + sticky = planner.mpc.get_vision_follow_cruise_hold( + "lead0", + lead_one, + lead_two, + 101.0, + 200.0, + cruise_obstacle, + v_ego, + t_follow, + tracking_lead, + ) + + assert sticky is None + + +@pytest.mark.parametrize("prev_source, lead_accel", [ + ("cruise", -0.02), + ("lead0", -0.60), +]) +def test_vision_follow_cruise_hold_never_delays_restrictive_transition(prev_source, lead_accel): + v_ego = 22.5 + t_follow = 1.20 + CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) + planner = LongitudinalPlanner(CP, init_v=v_ego) + lead_one = make_lead(status=True, d_rel=32.0, v_lead=22.0, a_lead=lead_accel, radar=False, model_prob=0.99) + lead_two = make_lead(status=False) + + sticky = planner.mpc.get_vision_follow_cruise_hold( + prev_source, + lead_one, + lead_two, + 101.0, + 200.0, + 100.0, + v_ego, + t_follow, + True, + ) + + assert sticky is None + + def test_near_duplicate_lead_source_hysteresis_prefers_previous_source_for_identical_radar_track(): v_ego = 27.0 CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) diff --git a/selfdrive/controls/tests/test_starpilot_vcruise.py b/selfdrive/controls/tests/test_starpilot_vcruise.py index 201a00a8e..a1e7e3c6f 100644 --- a/selfdrive/controls/tests/test_starpilot_vcruise.py +++ b/selfdrive/controls/tests/test_starpilot_vcruise.py @@ -100,6 +100,54 @@ def test_active_slc_control_target_does_not_require_set_speed_limit(): assert target == pytest.approx((48.0 * CV.MPH_TO_MS) - 0.4) +def test_curve_speed_controller_holds_target_through_brief_detector_dropout(): + planner, vcruise = make_vcruise() + sm = make_sm(standstill=False) + toggles = make_toggles() + toggles.curve_speed_controller = True + + def set_curve_target(_v_ego): + vcruise.csc.target_set = True + vcruise.csc.target = 14.0 + + vcruise.csc.update_target = set_curve_target + planner.road_curvature_detected = True + result = update_vcruise(vcruise, sm, toggles, now=10.0, v_ego=20.0) + assert result == pytest.approx(14.0) + assert vcruise.csc_controlling_speed + + planner.road_curvature_detected = False + result = update_vcruise(vcruise, sm, toggles, now=10.25, v_ego=20.0) + assert result == pytest.approx(14.0) + assert vcruise.csc_controlling_speed + + result = update_vcruise(vcruise, sm, toggles, now=10.8, v_ego=20.0) + assert result == pytest.approx(20.0) + assert not vcruise.csc_controlling_speed + + +def test_curve_speed_controller_releases_immediately_when_disabled(): + planner, vcruise = make_vcruise() + sm = make_sm(standstill=False) + toggles = make_toggles() + toggles.curve_speed_controller = True + + def set_curve_target(_v_ego): + vcruise.csc.target_set = True + vcruise.csc.target = 14.0 + + vcruise.csc.update_target = set_curve_target + planner.road_curvature_detected = True + update_vcruise(vcruise, sm, toggles, now=20.0, v_ego=20.0) + assert vcruise.csc_controlling_speed + + planner.road_curvature_detected = False + toggles.curve_speed_controller = False + result = update_vcruise(vcruise, sm, toggles, now=20.1, v_ego=20.0) + assert result == pytest.approx(20.0) + assert not vcruise.csc_controlling_speed + + def test_active_slc_control_target_applies_offset_and_cluster_diff(): target = get_active_slc_control_target( speed_limit_controller=True, @@ -286,7 +334,7 @@ def test_force_stop_turn_scene_veto_blocks_new_activation(): _, vcruise = make_vcruise(red_light=True, raw_model_stopped=False, forcing_stop=False) sm = make_sm(standstill=False) sm["carState"].leftBlinker = True - sm["carState"].steeringAngleDeg = 15.0 + sm["carState"].steeringAngleDeg = 30.0 result = update_vcruise(vcruise, sm, make_toggles(), now=0.0, v_ego=7.0) @@ -321,17 +369,17 @@ def test_force_stop_still_activates_for_straight_red_light_approach(): assert vcruise.forcing_stop -def test_force_stop_turn_scene_clears_moving_commitment(): +def test_force_stop_turn_scene_does_not_abandon_moving_commitment(): _, vcruise = make_vcruise(red_light=False, raw_model_stopped=False, forcing_stop=True) sm = make_sm(standstill=False) sm["carState"].rightBlinker = True - sm["carState"].steeringAngleDeg = -15.0 + sm["carState"].steeringAngleDeg = -30.0 result = update_vcruise(vcruise, sm, make_toggles(), now=0.0, v_ego=8.0) - assert result == pytest.approx(20.0) - assert vcruise.force_stop_timer == pytest.approx(0.0) - assert not vcruise.forcing_stop + assert result == pytest.approx(0.0) + assert vcruise.force_stop_timer >= 0.5 + assert vcruise.forcing_stop def test_engage_while_already_stopped_in_red_light_scene_seeds_force_stop_hold(): diff --git a/starpilot/controls/lib/conditional_experimental_mode.py b/starpilot/controls/lib/conditional_experimental_mode.py index a4f5f2b90..dedb7b94f 100644 --- a/starpilot/controls/lib/conditional_experimental_mode.py +++ b/starpilot/controls/lib/conditional_experimental_mode.py @@ -55,6 +55,7 @@ class ConditionalExperimentalMode: SLOW_LEAD_CONTINUITY_MIN_EGO = 2.5 SLOW_LEAD_CONTINUITY_HOLD_TIME = 1.25 SLOW_LEAD_FORCE_CLEAR_TIME = 0.75 + SLOW_LEAD_MODE_RELEASE_HOLD_TIME = 1.5 SLOW_LEAD_MIN_CLOSING_SPEED = 0.75 SLOW_LEAD_CLEAR_FASTER_FACTOR = 0.5 POST_STOP_LAUNCH_TRIGGER_SUPPRESS_TIME = 2.0 @@ -104,6 +105,7 @@ class ConditionalExperimentalMode: self.prev_experimental_mode = False # For hysteresis self.mode_hold_until = 0.0 self.mode_false_since = 0.0 + self.slow_lead_mode_hold_until = 0.0 self._prev_ce_status = None self.prev_standstill = False self.prev_standstill_stop_hold = False @@ -119,6 +121,7 @@ class ConditionalExperimentalMode: self.post_stop_launch_trigger_suppress_until = now + self.POST_STOP_LAUNCH_TRIGGER_SUPPRESS_TIME self.mode_hold_until = 0.0 self.mode_false_since = 0.0 + self.slow_lead_mode_hold_until = 0.0 self.prev_experimental_mode = False if not standstill: @@ -133,6 +136,10 @@ class ConditionalExperimentalMode: if triggered: self.mode_hold_until = now + self.CEM_TRANSITION_GUARD_TIME self.mode_false_since = 0.0 + if self.status_value == CEStatus["LEAD"]: + self.slow_lead_mode_hold_until = now + self.SLOW_LEAD_MODE_RELEASE_HOLD_TIME + else: + self.slow_lead_mode_hold_until = 0.0 elif self.prev_experimental_mode and self.mode_false_since == 0.0: self.mode_false_since = now elif not self.prev_experimental_mode: @@ -140,8 +147,17 @@ class ConditionalExperimentalMode: hold_active = now < self.mode_hold_until transition_buffer_active = self.mode_false_since != 0.0 and (now - self.mode_false_since) < self.CEM_TRANSITION_BUFFER_TIME + slow_lead_hold_active = bool( + starpilot_toggles.conditional_lead and + now < self.slow_lead_mode_hold_until and + self.has_credible_slow_lead_context(v_ego) + ) + if slow_lead_hold_active and not triggered: + self.status_value = CEStatus["LEAD"] + elif not slow_lead_hold_active: + self.slow_lead_mode_hold_until = 0.0 - self.experimental_mode = triggered or hold_active or transition_buffer_active + self.experimental_mode = triggered or slow_lead_hold_active or hold_active or transition_buffer_active self.prev_experimental_mode = self.experimental_mode ce_write_value = self.status_value if self.experimental_mode else CEStatus["OFF"] if ce_write_value != self._prev_ce_status: @@ -150,6 +166,7 @@ class ConditionalExperimentalMode: elif not is_manual_ce_status(self.status_value): self.mode_hold_until = 0.0 self.mode_false_since = 0.0 + self.slow_lead_mode_hold_until = 0.0 # Keep the stop-light path live at standstill so EXP stays pinned for a red # light / stop sign. Stop signs latch until pedal, while stop lights can @@ -168,6 +185,7 @@ class ConditionalExperimentalMode: else: self.mode_hold_until = 0.0 self.mode_false_since = 0.0 + self.slow_lead_mode_hold_until = 0.0 self._prev_ce_status = None self.experimental_mode = self.status_value == CEStatus["USER_OVERRIDDEN"] self.prev_experimental_mode = self.experimental_mode @@ -177,6 +195,19 @@ class ConditionalExperimentalMode: self.prev_standstill = standstill self.prev_standstill_stop_hold = current_standstill_stop_hold + def has_credible_slow_lead_context(self, v_ego): + lead = self.starpilot_planner.lead_one + if lead is None or not bool(getattr(lead, "status", False)): + return False + + lead_radar = bool(getattr(lead, "radar", False)) + lead_prob = float(getattr(lead, "modelProb", 1.0 if lead_radar else 0.0)) + if not lead_radar and lead_prob < self.SLOW_LEAD_CONTINUITY_MIN_MODEL_PROB: + return False + + lead_distance = float(getattr(lead, "dRel", float("inf"))) + return lead_distance < max(40.0, float(v_ego) * self.SLOW_LEAD_CONTINUITY_MAX_DISTANCE_TIME) + def get_standstill_stop_hold(self, sm): dash_stop_sign = ( bool(getattr(self.starpilot_planner.starpilot_vcruise, "stop_sign_confirmed", False)) or diff --git a/starpilot/controls/lib/starpilot_vcruise.py b/starpilot/controls/lib/starpilot_vcruise.py index fbc872b9a..9d11b6cd4 100644 --- a/starpilot/controls/lib/starpilot_vcruise.py +++ b/starpilot/controls/lib/starpilot_vcruise.py @@ -10,6 +10,7 @@ from openpilot.starpilot.controls.lib.curve_speed_controller import CurveSpeedCo from openpilot.starpilot.controls.lib.speed_limit_controller import SpeedLimitController CSC_MIN_SPEED = CITY_SPEED_LIMIT * CV.MPH_TO_MS +CSC_CURVE_RELEASE_HOLD_TIME = 0.75 OVERRIDE_FORCE_STOP_TIMER = 10 STANDSTILL_FORCE_STOP_CLEAR_TIME = 0.75 STANDSTILL_FORCE_STOP_LIGHT_HOLD_TIME = 5.0 @@ -148,6 +149,9 @@ class StarPilotVCruise: self._nav_instruction_state_raw = None self._nav_instruction_state = {} self._applied_slc_control_target = 0.0 + self.csc_controlling_speed = False + self.csc_target = 0.0 + self.csc_curve_last_seen_at = None def _update_nav_instruction_state(self): raw = self.starpilot_planner.params_memory.get("NavInstructionState") or {} @@ -399,19 +403,29 @@ class StarPilotVCruise: v_ego_diff = v_ego_cluster - v_ego # FrogsGoMoo's Curve Speed Controller - if long_control_active and v_ego > CRUISING_SPEED and self.starpilot_planner.road_curvature_detected and starpilot_toggles.curve_speed_controller: + csc_available = long_control_active and v_ego > CRUISING_SPEED and starpilot_toggles.curve_speed_controller + csc_curve_detected = csc_available and self.starpilot_planner.road_curvature_detected + if csc_curve_detected: self.csc.update_target(v_ego) self.csc_controlling_speed = True - self.csc_target = self.csc.target + self.csc_curve_last_seen_at = now else: - self.csc.log_data(v_ego, sm) + csc_release_hold = bool( + csc_available and + self.csc_controlling_speed and + self.csc_curve_last_seen_at is not None and + self._elapsed_seconds(now, self.csc_curve_last_seen_at) < CSC_CURVE_RELEASE_HOLD_TIME + ) + if not csc_release_hold: + self.csc.log_data(v_ego, sm) - self.csc_controlling_speed = False - self.csc.target_set = False + self.csc_controlling_speed = False + self.csc.target_set = False + self.csc_curve_last_seen_at = None - self.csc_target = v_cruise + self.csc_target = v_cruise # Pfeiferj's Speed Limit Controller self.slc.starpilot_toggles = starpilot_toggles