fix cluster

selfdrive/carrot/cluster/README.md

@@ -192,9 +192,10 @@ debug UI before navi data has arrived. When output is gated off,
 remain visible.
 The autorun watcher normalizes locale before this dim-only USB path too, so
 vendor USB initialization does not fail before the renderer is launched.
-Manager autostart leaves realtime affinity off by default. If `CLUSTER_REALTIME=1`
-is set, `cluster_autorun.py` uses `ClusterHudCoreMode=0` by default, which maps
-to cores `1,2,3,4`; mode `1` maps to all initially allowed CPU cores.
+Manager autostart sets `CLUSTER_REALTIME=1` by default unless the environment
+already overrides it. With realtime enabled, `cluster_autorun.py` uses
+`ClusterHudCoreMode=0` by default, which maps to cores `1,2,3,4`; mode `1` maps
+to all initially allowed CPU cores.
 `ClusterHudPriority` controls the common openpilot realtime helper priority with
 range `1..99`, default `10`.
 Changing either param makes the running HUD exit so `cluster_autorun` can
@@ -328,6 +329,8 @@ reflections merge cleanly.
 vehicles yellow, `radarState` front/SCC radar leads red, camera-sourced vehicle
 leads light blue, comma model leads dark blue, and ADAS corner detections from
 `0x162`/`0x1EA` green.
+Radar samples whose distance and left/right offset are both zero are treated as
+empty/default data and are not drawn as radar points or vehicle boxes.
 Radar-track vehicle classification rejects points outside model road edges, but
 does not require in-road points to sit near the road-edge line; center-lane
 points can classify as vehicles when probability/in-lane data or moving radar
@@ -348,6 +351,13 @@ The planned path draws `longitudinalPlan.desiredDistance` as a magenta
 horizontal bar across the current lane width at the matching forward position.
 Changing `ClusterHud` to another supported mode or `0` makes the running HUD
 exit; cleanup sends TURZX brightness zero before releasing the USB device.
+When autorun passes a HUD mode, USB open is pinned to that mode's TURZX PID
+(`1 -> 0x0092`, `2 -> 0x0123`) so a second connected TURZX panel is not opened
+by the vendor library's generic device scan.
+If frame or H264 chunk writes report that the USB device was disconnected, the
+active HUD exits instead of trying to recover in-process. Autorun calls the
+launcher in non-exiting mode so the error returns to the watcher loop, letting
+`cluster_autorun` wait for the same PID and relaunch after replug.
 
 The bundled TURZX code includes only the Python vendor library. The openpilot
 device uses the system `libusb-1.0.so` through `pyusb`.

selfdrive/carrot/cluster/cluster_models.py

@@ -150,6 +150,13 @@ class RadarPoint:
     promotion_held: bool = False
 
 
+RADAR_ZERO_POSITION_EPS_M = 1e-3
+
+
+def radar_position_is_zero(longitudinal_m: float, lateral_m: float) -> bool:
+    return abs(longitudinal_m) <= RADAR_ZERO_POSITION_EPS_M and abs(lateral_m) <= RADAR_ZERO_POSITION_EPS_M
+
+
 @dataclass(frozen=True, slots=True)
 class SimulatorInput:
     throttle: float = 0.0

selfdrive/carrot/cluster/cluster_route_replay.py

@@ -40,6 +40,7 @@
     ModelRiskPoint,
     RadarPoint,
     RouteOverlay,
+    radar_position_is_zero,
 )
 from cluster_utils import clamp, smoothstep
 
@@ -1456,6 +1457,8 @@ def _update_radar_state(self, radar_state: Any, event_t: float) -> None:
                 continue
             # openpilot yRel is left-positive; this renderer uses right-positive x.
             lateral_m = -safe_float(lead, "yRel", 0.0)
+            if radar_position_is_zero(d_rel, lateral_m):
+                continue
             relative_speed_mps = safe_optional_float(lead, "vRel")
             lead_speed_mps = safe_optional_float(lead, "vLead")
             absolute_speed_kph = (
@@ -2994,6 +2997,8 @@ def live_track_to_radar_point(track: Any, index: int, ego_speed_kph: float) -> R
         return None
     y_rel = safe_float(track, "yRel", 0.0)
     lateral_m = renderer_lateral_from_openpilot_yrel(y_rel)
+    if radar_position_is_zero(d_rel, lateral_m):
+        return None
     if not -12.0 <= lateral_m <= 12.0:
         return None
     track_id = safe_optional_int(track, "trackId")
@@ -3029,6 +3034,7 @@ def sorted_radar_points(points: Any) -> tuple[RadarPoint, ...]:
         for point in points
         if -12.0 <= point.lateral_m <= 12.0
         and RADAR_MIN_LONGITUDINAL_M <= point.longitudinal_m <= RADAR_FRONT_MAX_LONGITUDINAL_M
+        and not radar_position_is_zero(point.longitudinal_m, point.lateral_m)
     ]
     filtered.sort(key=lambda point: (point.longitudinal_m, abs(point.lateral_m), point.label))
     return tuple(filtered)

selfdrive/carrot/cluster/cluster_scene.py

@@ -41,7 +41,14 @@
     VEHICLE_LENGTH_M,
     VEHICLE_WIDTH_M,
 )
-from cluster_models import ClusterUiState, DetectedVehicle, LaneMarking, ModelPathPoint, RadarPoint
+from cluster_models import (
+    ClusterUiState,
+    DetectedVehicle,
+    LaneMarking,
+    ModelPathPoint,
+    RadarPoint,
+    radar_position_is_zero,
+)
 from cluster_utils import clamp, darken, lighten, smoothstep
 
 
@@ -1636,9 +1643,35 @@ def path_metric_color(accel_mps2: float) -> Color:
 
 
 def radar_points_for_display(state: ClusterUiState) -> tuple[RadarPoint, ...]:
+    points = state.radar_points
+    if any(radar_position_is_zero(point.longitudinal_m, point.lateral_m) for point in points):
+        points = tuple(
+            point
+            for point in points
+            if not radar_position_is_zero(point.longitudinal_m, point.lateral_m)
+        )
     if state.radar_display_mode == CLUSTER_RADAR_DISPLAY_DETAIL:
-        return state.radar_points
-    return merged_radar_points(state.radar_points, state)
+        return points
+    return merged_radar_points(points, state)
+
+
+def detected_vehicle_is_zero_radar_sample(vehicle: DetectedVehicle) -> bool:
+    if not radar_position_is_zero(vehicle.longitudinal_m, vehicle.lateral_m):
+        return False
+    return (
+        vehicle.source == "radarState"
+        or vehicle.source == "carState"
+        or vehicle.source in ("radarPoint", "liveTracks")
+        or vehicle.source.startswith("CAN 0x")
+    )
+
+
+def detected_vehicles_without_zero_radar_samples(
+    vehicles: tuple[DetectedVehicle, ...],
+) -> tuple[DetectedVehicle, ...]:
+    if not any(detected_vehicle_is_zero_radar_sample(vehicle) for vehicle in vehicles):
+        return vehicles
+    return tuple(vehicle for vehicle in vehicles if not detected_vehicle_is_zero_radar_sample(vehicle))
 
 
 def merged_radar_points(points: tuple[RadarPoint, ...], state: ClusterUiState) -> tuple[RadarPoint, ...]:
@@ -1865,6 +1898,7 @@ def detected_vehicles_for_display(
     vehicles: tuple[DetectedVehicle, ...],
     state: ClusterUiState,
 ) -> tuple[DetectedVehicle, ...]:
+    vehicles = detected_vehicles_without_zero_radar_samples(vehicles)
     if state.radar_display_mode == CLUSTER_RADAR_DISPLAY_DETAIL or len(vehicles) < 2:
         return vehicles
     selected: list[DetectedVehicle] = []
@@ -3059,8 +3093,9 @@ def build_cluster_scene(
     profile_stage = profile_scene_start(profile_add)
     lane_width_m = max(2.4, min(4.6, state.lane_width_m or DEFAULT_LANE_WIDTH_M))
     display_radar_points = radar_points_for_display(state)
-    if display_radar_points is not state.radar_points:
-        state = replace(state, radar_points=display_radar_points)
+    display_detected_vehicles = detected_vehicles_without_zero_radar_samples(state.detected_vehicles)
+    if display_radar_points is not state.radar_points or display_detected_vehicles != state.detected_vehicles:
+        state = replace(state, radar_points=display_radar_points, detected_vehicles=display_detected_vehicles)
     anchor_x_m = ego_anchor_x_m(state, lane_width_m)
     scene_shift_x_m = -anchor_x_m
     relative_scene_x_offset_m = -scene_shift_x_m

selfdrive/carrot/cluster/cluster_usb_display.py

@@ -1,5 +1,6 @@
 from __future__ import annotations
 
+import errno
 import sys
 import os
 import threading
@@ -27,6 +28,22 @@
 USB_FRAME_TIMEOUT_MS = 2000
 USB_COMMAND_GAP_S = 0.2
 TURZX_BRIGHTNESS_COMMAND_MAX = 102
+USB_DISCONNECT_ERRNOS = {
+    errno.ENODEV,
+    errno.ENXIO,
+    errno.EIO,
+    getattr(errno, "ESHUTDOWN", 108),
+}
+USB_DISCONNECT_BACKEND_CODES = {
+    -4,  # libusb: LIBUSB_ERROR_NO_DEVICE
+}
+USB_DISCONNECT_TEXT = (
+    "no such device",
+    "device not found",
+    "disconnected",
+    "entity not found",
+    "device has been disconnected",
+)
 CMD_GET_H264_CHUNK_SIZE = 17
 CMD_PLAY_H264_CHUNK = 121
 CMD_GET_STREAM_STATUS = 122
@@ -101,11 +118,13 @@ def __init__(
         frame_drain_timeout_ms: int = 2,
         fast_frame_drain_attempts: int = 3,
         fast_frame_drain_timeout_ms: int = 2,
+        expected_product_id: int | None = None,
     ) -> None:
         self.brightness = int(clamp(brightness, 0, 100))
         self.display_fps = int(clamp(display_fps, 0, 255))
         self.jpeg_quality = int(clamp(jpeg_quality, 1, 95))
         self.jpeg_encoder = jpeg_encoder
+        self.expected_product_id = expected_product_id
         self.fast_write = fast_write
         self.wait_for_frame_ack = wait_for_frame_ack
         self.frame_drain_attempts = max(0, int(frame_drain_attempts))
@@ -131,6 +150,7 @@ def __init__(
         self._ep_out = None
         self._ep_in = None
         self._dll_dir_handle = None
+        self._disconnected = False
         self._frame_error_count = 0
         self._turbojpeg = None
         self._turbojpeg_unavailable = False
@@ -203,10 +223,11 @@ def close(self) -> None:
             self._ep_in = None
             return
 
-        try:
-            self._send_brightness(0, "brightness-off")
-        except Exception as exc:
-            print(f"Warning: TURZX USB brightness-off command skipped during close: {exc}", flush=True)
+        if not self._disconnected:
+            try:
+                self._send_brightness(0, "brightness-off")
+            except Exception as exc:
+                print(f"Warning: TURZX USB brightness-off command skipped during close: {exc}", flush=True)
 
         try:
             import usb.util
@@ -231,8 +252,35 @@ def set_brightness(self, brightness: int, *, force: bool = False) -> bool:
             return True
         return False
 
+    def _find_expected_usb_device(self) -> tuple[Any, int]:
+        if self.expected_product_id is None:
+            return self._find_usb_device()
+
+        import usb.core  # type: ignore
+
+        dev = usb.core.find(idVendor=TURZX_USB_VENDOR_ID, idProduct=self.expected_product_id)
+        if dev is None:
+            raise ValueError(f"USB device not found for pid=0x{self.expected_product_id:04x}")
+
+        try:
+            dev.set_configuration()
+        except usb.core.USBError as exc:
+            print("Warning: set_configuration() failed:", exc)
+
+        if sys.platform.startswith("linux"):
+            try:
+                if dev.is_kernel_driver_active(0):
+                    dev.detach_kernel_driver(0)
+            except usb.core.USBError as exc:
+                print("Warning: detach_kernel_driver failed:", exc)
+
+        return dev, self.expected_product_id
+
     def _connect_device(self) -> None:
-        self.dev, self.dev_pid = self._find_usb_device()
+        self.dev, self.dev_pid = self._find_expected_usb_device()
+        self._disconnected = False
+        if self.dev_pid not in self._product_id:
+            raise RuntimeError(f"TURZX vendor library does not know pid=0x{self.dev_pid:04x}")
         self._cache_out_endpoint()
         portrait_width, portrait_height = self._product_id[self.dev_pid]
         self.landscape_width = portrait_height
@@ -352,6 +400,37 @@ def _reset_and_reconnect(self) -> None:
         time.sleep(1.5)
         self._connect_device()
 
+    @staticmethod
+    def _exception_indicates_disconnect(exc: BaseException) -> bool:
+        visited: set[int] = set()
+        current: BaseException | None = exc
+        while current is not None and id(current) not in visited:
+            visited.add(id(current))
+            error_no = getattr(current, "errno", None)
+            if error_no in USB_DISCONNECT_ERRNOS:
+                return True
+            backend_error_code = getattr(current, "backend_error_code", None)
+            if backend_error_code in USB_DISCONNECT_BACKEND_CODES:
+                return True
+            text = str(current).lower()
+            if any(pattern in text for pattern in USB_DISCONNECT_TEXT):
+                return True
+            current = current.__cause__ or current.__context__
+        return False
+
+    def _mark_disconnected(self) -> None:
+        self.dev = None
+        self.dev_pid = None
+        self._ep_out = None
+        self._ep_in = None
+        self._disconnected = True
+
+    def _raise_usb_error(self, error_message: str, exc: BaseException) -> None:
+        if self._exception_indicates_disconnect(exc):
+            self._mark_disconnected()
+            raise RuntimeError("TURZX USB display disconnected") from exc
+        raise RuntimeError(error_message) from exc
+
     def _add_libusb_search_path(self) -> None:
         libusb = VENDOR_ROOT / "external" / "libusb-1.0" / "libusb-1.0.dll"
         if not libusb.exists():
@@ -429,7 +508,9 @@ def send_h264_chunk(
             if wait_for_ack:
                 try:
                     response = self._send_h264_chunk_ack(chunk, is_last=is_last)
-                except Exception:
+                except Exception as exc:
+                    if self._exception_indicates_disconnect(exc):
+                        raise
                     if require_ack_response:
                         raise
                     self._h264_flow_control(target_queue_depth=2)
@@ -586,7 +667,7 @@ def _write_payload_checked(self, payload: bytes, error_message: str, timeout_ms:
                 self._profile_add("usb.write_checked.read_ack", profile_stage)
                 return response
             except Exception as exc:
-                raise RuntimeError(error_message) from exc
+                self._raise_usb_error(error_message, exc)
 
     def _write_payload_no_ack(self, payload: bytes, error_message: str, timeout_ms: int) -> None:
         if self._ep_out is None:
@@ -601,7 +682,7 @@ def _write_payload_no_ack(self, payload: bytes, error_message: str, timeout_ms:
                 self._ep_out.write(payload, timeout_ms)
                 self._profile_add("usb.write_no_ack.write", profile_stage)
             except Exception as exc:
-                raise RuntimeError(error_message) from exc
+                self._raise_usb_error(error_message, exc)
 
     def _build_frame_payload(self, command_id: int, frame: bytes) -> bytes:
         if self._build_command_packet_header is None or self._encrypt_command_packet is None:
@@ -735,7 +816,7 @@ def _send_h264_chunk_ack(self, chunk: bytes, *, is_last: bool) -> bytes:
                 self._profile_add("usb.h264.read_ack", profile_stage)
                 return response
             except Exception as exc:
-                raise RuntimeError("TURZX USB H264 chunk timed out") from exc
+                self._raise_usb_error("TURZX USB H264 chunk timed out", exc)
 
     def _h264_flow_control(self, *, target_queue_depth: int, max_attempts: int = 8) -> None:
         for _ in range(max_attempts):
@@ -779,14 +860,18 @@ def _send_h264_chunk_no_ack(self, chunk: bytes, *, is_last: bool, drain_input: b
                 self._ep_out.write(payload, USB_FRAME_TIMEOUT_MS)
                 self._profile_add("usb.h264_no_ack.write", profile_stage)
             except Exception as exc:
-                raise RuntimeError("TURZX USB H264 chunk write failed") from exc
+                self._raise_usb_error("TURZX USB H264 chunk write failed", exc)
 
     def _check_frame_response(self, response: bytes | None) -> None:
         if not response:
             raise RuntimeError("TURZX USB frame upload timed out")
         self._frame_error_count = 0
 
     def _handle_frame_error(self, exc: Exception) -> None:
+        if self._exception_indicates_disconnect(exc):
+            self._mark_disconnected()
+            raise RuntimeError("TURZX USB display disconnected") from exc
+
         self._frame_error_count += 1
         print(
             f"USB frame upload failed "

selfdrive/carrot/cluster/main.py

@@ -65,7 +65,7 @@
 from cluster_route_replay import RouteReplaySource
 from cluster_simulator import ClusterSimulator, RandomInputSource
 from cluster_system_monitor import ClusterProcessCoreUsageSampler
-from cluster_usb_display import TuringUsbDisplay
+from cluster_usb_display import TuringUsbDisplay, product_id_for_hud_mode
 from cluster_usb_pipeline import AsyncJpegUsbPipeline
 
 DEFAULT_FPS = 0.0
@@ -584,6 +584,9 @@ def request_stop(signum: int, _frame: object) -> None:
             frame_drain_timeout_ms=usb_frame_drain_timeout_ms,
             fast_frame_drain_attempts=usb_fast_drain_attempts,
             fast_frame_drain_timeout_ms=usb_fast_drain_timeout_ms,
+            expected_product_id=(
+                product_id_for_hud_mode(hud_mode_watch) if hud_mode_watch is not None else None
+            ),
         )
         usb_display.set_profile_enabled(profile_render)
         profile_stage = time.perf_counter()
@@ -1667,7 +1670,7 @@ def parse_args() -> argparse.Namespace:
     return args
 
 
-def main() -> None:
+def main(*, exit_on_error: bool = True) -> None:
     args = parse_args()
     encoder_source = apply_cluster_encoder_param(args)
     if args.usb_async and args.usb_codec != "jpeg":
@@ -1811,6 +1814,8 @@ def main() -> None:
     except KeyboardInterrupt:
         print("\nStopped.")
     except RuntimeError as exc:
+        if not exit_on_error:
+            raise
         raise SystemExit(f"Error: {exc}") from exc