From ef400a9b3f5d6b5b09a4036549b67605e5a9b0d0 Mon Sep 17 00:00:00 2001 From: firestar5683 <168790843+firestar5683@users.noreply.github.com> Date: Tue, 21 Jul 2026 17:15:10 -0500 Subject: [PATCH] Same Subscriber --- .../tests/test_speed_limit_controller.py | 47 +++++++++++++++++ .../controls/lib/speed_limit_controller.py | 14 ++++- starpilot/system/speed_limit_vision.py | 49 ++++------------- .../system/tests/test_speed_limit_vision.py | 52 ++++++++----------- 4 files changed, 92 insertions(+), 70 deletions(-) diff --git a/selfdrive/controls/tests/test_speed_limit_controller.py b/selfdrive/controls/tests/test_speed_limit_controller.py index bc6bd3937..410294b27 100644 --- a/selfdrive/controls/tests/test_speed_limit_controller.py +++ b/selfdrive/controls/tests/test_speed_limit_controller.py @@ -21,6 +21,15 @@ class FakeParams: def get_float(self, key): return float(self.values.get(key, 0) or 0) + def get_int(self, key): + return int(self.values.get(key, 0) or 0) + + def put_float(self, key, value): + self.values[key] = value + + def put_int(self, key, value): + self.values[key] = value + def put_nonblocking(self, key, value): self.values[key] = value @@ -86,6 +95,44 @@ def mph(value): return value * CV.MPH_TO_MS +def test_large_vision_delta_requires_three_detector_frames(): + controller = make_controller( + speed_limit_priority1="Vision", + vision_speed_limit_detection=True, + ) + try: + controller.starpilot_planner.params_memory.put_float("VisionSpeedLimit", mph(15)) + controller.starpilot_planner.params_memory.put_float("VisionSpeedLimitSupportSpeed", mph(15)) + controller.starpilot_planner.params_memory.put_int("VisionSpeedLimitSupportCount", 2) + sm = make_sm(gas_pressed=False) + + controller.update_limits(0.0, datetime.now(timezone.utc), False, mph(75), mph(70), sm) + assert controller.target == 0 + + controller.starpilot_planner.params_memory.put_int("VisionSpeedLimitSupportCount", 3) + controller.update_limits(0.0, datetime.now(timezone.utc), False, mph(75), mph(70), sm) + assert controller.target == pytest.approx(mph(15)) + assert controller.source == "Vision" + finally: + controller.shutdown() + + +def test_normal_vision_delta_keeps_fast_path(): + controller = make_controller( + speed_limit_priority1="Vision", + vision_speed_limit_detection=True, + ) + try: + controller.starpilot_planner.params_memory.put_float("VisionSpeedLimit", mph(50)) + sm = make_sm(gas_pressed=False) + + controller.update_limits(0.0, datetime.now(timezone.utc), False, mph(75), mph(70), sm) + assert controller.target == pytest.approx(mph(50)) + assert controller.source == "Vision" + finally: + controller.shutdown() + + def test_new_source_limit_clears_override_until_gas_release(): controller = make_controller() try: diff --git a/starpilot/controls/lib/speed_limit_controller.py b/starpilot/controls/lib/speed_limit_controller.py index 322d8530f..6bdd69f78 100644 --- a/starpilot/controls/lib/speed_limit_controller.py +++ b/starpilot/controls/lib/speed_limit_controller.py @@ -36,6 +36,9 @@ OFFSET_MAP_METRIC = [ ] SLC_OVERRIDE_DISABLE_CLEAR_TIME = 0.75 +VISION_LARGE_SET_SPEED_DELTA = 30 * CV.MPH_TO_MS +VISION_LARGE_SET_SPEED_MIN_SUPPORT = 3 +VISION_SUPPORT_SPEED_TOLERANCE = 0.5 * CV.MPH_TO_MS class SpeedLimitController: def __init__(self, StarPilotVCruise): @@ -326,7 +329,14 @@ class SpeedLimitController: def update_limits(self, dashboard_speed_limit, now, time_validated, v_cruise, v_ego, sm, display_only=False): self.update_map_speed_limit(v_ego, sm) - self.vision_limit = self.starpilot_planner.params_memory.get_float("VisionSpeedLimit") if getattr(self.starpilot_toggles, "vision_speed_limit_detection", False) else 0 + vision_enabled = getattr(self.starpilot_toggles, "vision_speed_limit_detection", False) + self.vision_limit = self.starpilot_planner.params_memory.get_float("VisionSpeedLimit") if vision_enabled else 0 + usable_vision_limit = self.vision_limit + if usable_vision_limit > 0 and v_cruise > 0 and abs(usable_vision_limit - v_cruise) >= VISION_LARGE_SET_SPEED_DELTA: + support_count = self.starpilot_planner.params_memory.get_int("VisionSpeedLimitSupportCount") + support_speed = self.starpilot_planner.params_memory.get_float("VisionSpeedLimitSupportSpeed") + if support_count < VISION_LARGE_SET_SPEED_MIN_SUPPORT or abs(support_speed - usable_vision_limit) > VISION_SUPPORT_SPEED_TOLERANCE: + usable_vision_limit = 0 configured_priorities = { self.starpilot_toggles.speed_limit_priority1, @@ -337,7 +347,7 @@ class SpeedLimitController: "Map Data": self.map_speed_limit, } if "Vision" in configured_priorities: - limits["Vision"] = self.vision_limit + limits["Vision"] = usable_vision_limit filtered_limits = {source: limit for source, limit in limits.items() if limit >= 1} if self.starpilot_toggles.speed_limit_priority_highest: diff --git a/starpilot/system/speed_limit_vision.py b/starpilot/system/speed_limit_vision.py index 7198b7902..012ca235d 100644 --- a/starpilot/system/speed_limit_vision.py +++ b/starpilot/system/speed_limit_vision.py @@ -15,7 +15,6 @@ import numpy as np from openpilot.common.constants import CV from openpilot.common.realtime import set_core_affinity -from openpilot.selfdrive.car.cruise import V_CRUISE_UNSET from openpilot.system.hardware import PC RUNTIME_LOOP_HZ = 20 @@ -63,8 +62,6 @@ CHANGE_REPEAT_MIN_CONFIDENCE = 0.70 LOW_SPEED_CHANGE_CONSISTENT_DETECTIONS = 2 LOW_SPEED_CHANGE_MIN_CONFIDENCE = 0.90 LOW_SPEED_CHANGE_ALLOW_STRONG_CONSENSUS = True -LARGE_SET_SPEED_DELTA_MPH = 30.0 -LARGE_SET_SPEED_DELTA_CONSISTENT_DETECTIONS = 3 MODEL_DETECTION_SHORT_CIRCUIT_CONFIDENCE = 0.65 PUBLISHED_HOLD_SECONDS = 300.0 PUBLISHED_CHANGE_COOLDOWN_SECONDS = 1.4 @@ -314,7 +311,7 @@ class SpeedLimitVisionDaemon: self.Ratekeeper = Ratekeeper self.VisionIpcClient = VisionIpcClient self.VisionStreamType = VisionStreamType - self.sm = messaging.SubMaster(["carState", "deviceState", "mapdOut", "userBookmark", "livePose"]) + self.sm = messaging.SubMaster(["deviceState", "mapdOut", "userBookmark", "livePose"]) self.client = None self.stream_name = "" @@ -340,7 +337,6 @@ class SpeedLimitVisionDaemon: self.started_prev = False self.history: deque[HistoryEntry] = deque() - self.current_cruise_set_speed_ms = 0.0 self.published_speed_limit_mph = 0 self.published_confidence = 0.0 self.previous_published_speed_limit_mph = 0 @@ -442,24 +438,6 @@ class SpeedLimitVisionDaemon: conversion = CV.KPH_TO_MS if self.is_metric else CV.MPH_TO_MS return speed_value * conversion - def _update_current_cruise_set_speed(self): - self.current_cruise_set_speed_ms = 0.0 - if self.sm is None: - return - try: - v_cruise_kph = float(self.sm["carState"].vCruise) - except Exception: - return - if math.isfinite(v_cruise_kph) and 0.0 < v_cruise_kph < V_CRUISE_UNSET: - self.current_cruise_set_speed_ms = v_cruise_kph * CV.KPH_TO_MS - - def _has_large_cruise_set_speed_delta(self, speed_limit): - cruise_set_speed_ms = getattr(self, "current_cruise_set_speed_ms", 0.0) - return ( - cruise_set_speed_ms > 0.0 and - abs(self._speed_value_to_ms(speed_limit) - cruise_set_speed_ms) >= LARGE_SET_SPEED_DELTA_MPH * CV.MPH_TO_MS - ) - def _speed_value_from_ms(self, speed_ms): conversion = CV.MS_TO_KPH if self.is_metric else CV.MS_TO_MPH return int(round(speed_ms * conversion)) if speed_ms > 0.0 else 0 @@ -2227,10 +2205,6 @@ class SpeedLimitVisionDaemon: has_strong_consensus = any(entry.strong_consensus for entry in matching_entries) current_speed_limit = self.published_speed_limit_mph current_count = counts.get(current_speed_limit, 0) if current_speed_limit > 0 else 0 - large_set_speed_delta = ( - candidate_speed_limit != current_speed_limit and - self._has_large_cruise_set_speed_delta(candidate_speed_limit) - ) if current_speed_limit > 0 and candidate_speed_limit != current_speed_limit: required_count = CHANGE_CONSISTENT_DETECTIONS @@ -2245,9 +2219,6 @@ class SpeedLimitVisionDaemon: ) if best_confidence < LOW_SPEED_CHANGE_MIN_CONFIDENCE: return None - if large_set_speed_delta: - required_count = max(required_count, LARGE_SET_SPEED_DELTA_CONSISTENT_DETECTIONS) - allow_single_frame_confirmation = False if candidate_count < required_count and not allow_single_frame_confirmation: return None if ( @@ -2259,13 +2230,6 @@ class SpeedLimitVisionDaemon: return None return candidate_speed_limit, best_confidence - if large_set_speed_delta: - if candidate_count < LARGE_SET_SPEED_DELTA_CONSISTENT_DETECTIONS: - return None - if matching_confidences[LARGE_SET_SPEED_DELTA_CONSISTENT_DETECTIONS - 1] < CHANGE_REPEAT_MIN_CONFIDENCE: - return None - return candidate_speed_limit, best_confidence - if has_strong_consensus or best_confidence >= STRONG_DETECTION_CONFIDENCE or candidate_count >= CONSISTENT_DETECTIONS: return candidate_speed_limit, best_confidence return None @@ -2288,6 +2252,14 @@ class SpeedLimitVisionDaemon: if self.params_memory is not None: self.params_memory.remove("VisionSpeedLimit") self.params_memory.remove("VisionSpeedLimitConfidence") + self.params_memory.remove("VisionSpeedLimitSupportCount") + self.params_memory.remove("VisionSpeedLimitSupportSpeed") + + def _publish_detection_support(self, speed_limit_mph, support_count): + if self.params_memory is None: + return + self.params_memory.put_int("VisionSpeedLimitSupportCount", support_count) + self.params_memory.put_float("VisionSpeedLimitSupportSpeed", self._speed_value_to_ms(speed_limit_mph)) def _publish_status(self, status, clear_speed=False): if clear_speed: @@ -2485,6 +2457,7 @@ class SpeedLimitVisionDaemon: confirmed = self._confirm_detection() if confirmed is not None: speed_limit_mph, confidence = confirmed + support_count = sum(entry.speed_limit_mph == speed_limit_mph for entry in self.history) if self._should_hold_current_publish(speed_limit_mph, confidence, now): self._publish_status( f"Candidate {self._format_speed_value(speed_limit_mph)} ({confidence * 100:.0f}%)", @@ -2492,6 +2465,7 @@ class SpeedLimitVisionDaemon: ) else: self._publish_detection(speed_limit_mph, confidence, "Holding") + self._publish_detection_support(speed_limit_mph, support_count) else: self._publish_status( f"Candidate {self._format_speed_value(detection.speed_limit_mph)} ({detection.confidence * 100:.0f}%)", @@ -2508,7 +2482,6 @@ class SpeedLimitVisionDaemon: now = time.monotonic() self.loop_count += 1 self.sm.update(0) - self._update_current_cruise_set_speed() if self.sm.updated["userBookmark"]: if self.ignore_next_user_bookmark: diff --git a/starpilot/system/tests/test_speed_limit_vision.py b/starpilot/system/tests/test_speed_limit_vision.py index 8c2b930f4..6e4460a04 100644 --- a/starpilot/system/tests/test_speed_limit_vision.py +++ b/starpilot/system/tests/test_speed_limit_vision.py @@ -16,6 +16,12 @@ class MemoryParams: def put_float(self, key, value): self.values[key] = value + def put_int(self, key, value): + self.values[key] = value + + def remove(self, key): + self.values.pop(key, None) + class StaticClassifierNet: def __init__(self, probabilities): @@ -30,8 +36,6 @@ class StaticClassifierNet: def daemon_with_history(current_speed, entries): daemon = SpeedLimitVisionDaemon.__new__(SpeedLimitVisionDaemon) - daemon.is_metric = False - daemon.current_cruise_set_speed_ms = 0.0 daemon.published_speed_limit_mph = current_speed daemon.history = deque(HistoryEntry(speed, confidence, float(index)) for index, (speed, confidence) in enumerate(entries)) return daemon @@ -143,6 +147,22 @@ def test_five_mph_detection_is_publishable(): assert detection.speed_limit_mph == 5 +def test_detection_support_counts_independent_frames(): + daemon = publishing_daemon(False) + daemon.followup_until = 0.0 + daemon.last_detection_at = 0.0 + daemon.last_candidate_speed_limit_mph = 0 + daemon.last_candidate_confidence = 0.0 + daemon.last_candidate_at = 0.0 + daemon.last_logged_candidate = None + daemon._schedule_training_capture = lambda *_args, **_kwargs: None + + for expected_count in range(1, 4): + daemon._update_detection(slv.Detection(15, 0.99)) + assert daemon.params_memory.values["VisionSpeedLimitSupportCount"] == expected_count + assert daemon.params_memory.values["VisionSpeedLimitSupportSpeed"] == pytest.approx(15 * slv.CV.MPH_TO_MS) + + def test_speed_change_requires_two_matching_reads_below_single_read_threshold(): daemon = daemon_with_history(40, [(55, 0.82)]) assert daemon._confirm_detection() is None @@ -191,34 +211,6 @@ def test_low_speed_change_rejects_low_confidence_sequence(): assert daemon._confirm_detection() is None -@pytest.mark.parametrize( - ("is_metric", "set_speed", "candidate_speed", "conversion"), - ( - (False, 75, 45, slv.CV.MPH_TO_MS), - (True, 100, 50, slv.CV.KPH_TO_MS), - ), -) -def test_large_cruise_set_speed_delta_requires_three_reads(is_metric, set_speed, candidate_speed, conversion): - daemon = daemon_with_history(0, [(candidate_speed, 0.99)]) - daemon.is_metric = is_metric - daemon.current_cruise_set_speed_ms = set_speed * conversion - assert daemon._has_large_cruise_set_speed_delta(candidate_speed) - assert daemon._confirm_detection() is None - - daemon.history.append(HistoryEntry(candidate_speed, 0.95, 1.0, strong_consensus=True)) - assert daemon._confirm_detection() is None - - daemon.history.append(HistoryEntry(candidate_speed, 0.91, 1.5)) - assert daemon._confirm_detection() == pytest.approx((candidate_speed, 0.99)) - - -def test_normal_cruise_set_speed_delta_keeps_single_read_confirmation(): - daemon = daemon_with_history(0, [(50, 0.99)]) - daemon.current_cruise_set_speed_ms = 75 * slv.CV.MPH_TO_MS - - assert daemon._confirm_detection() == pytest.approx((50, 0.99)) - - def textured_track_frame(offset_x=0, offset_y=0): frame = np.zeros((120, 180, 3), dtype=np.uint8) x1, y1, x2, y2 = 90 + offset_x, 30 + offset_y, 130 + offset_x, 90 + offset_y