mawei/StarPilot
1
0
Fork 0
mirror of https://github.com/firestar5683/StarPilot.git synced 2026-07-24 18:52:09 +08:00
Code Issues Packages Projects Releases Wiki Activity

Same Subscriber

Browse Source
This commit is contained in:
firestar5683
2026-07-21 17:15:10 -05:00
parent d4b3c3c3b0
commit ef400a9b3f
4 changed files with 92 additions and 70 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/controls/tests/test_speed_limit_controller.py
+47
View File
@@ -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:
starpilot/controls/lib/speed_limit_controller.py
+12 -2
View File
@@ -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:
starpilot/system/speed_limit_vision.py
+11 -38
View File
@@ -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:
starpilot/system/tests/test_speed_limit_vision.py
+22 -30
View File
@@ -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