Handling correct camera disconnection on startup and during runtime

synapse_core/src/synapse/core/camera_factory.py

@@ -4,7 +4,6 @@
 # SPDX-License-Identifier: GPL-3.0-or-later
 
 import queue
-import socket
 import threading
 import time
 from abc import ABC, abstractmethod
@@ -159,6 +158,76 @@ def __init__(self, name: str) -> None:
         self.name: str = name
         self.stream: str = ""
         self.cameraIndex: CameraID = -1
+        self.isRunning: bool = True
+
+    def generateNoSignalFrame(self, size: Resolution = (640, 480)) -> Frame:
+        width, height = size
+
+        frame = np.zeros((height, width, 3), dtype=np.uint8)
+
+        colors = [
+            (255, 255, 255),  # white
+            (0, 255, 255),  # yellow
+            (255, 255, 0),  # cyan
+            (0, 255, 0),  # green
+            (255, 0, 255),  # magenta
+            (0, 0, 255),  # red
+            (255, 0, 0),  # blue
+        ]
+
+        bar_width = width // len(colors)
+        for i, color in enumerate(colors):
+            frame[:, i * bar_width : (i + 1) * bar_width] = color
+
+        noise_intensity = np.random.randint(10, 40)
+        noise = np.random.randint(0, noise_intensity, frame.shape, dtype=np.uint8)
+        frame = cv2.add(frame, noise)
+
+        for y in range(0, height, 2):
+            frame[y : y + 1, :] = (frame[y : y + 1, :] * 0.6).astype(np.uint8)
+
+        if np.random.rand() > 0.7:
+            glitch_y = np.random.randint(0, height)
+            glitch_height = np.random.randint(5, 20)
+            shift = np.random.randint(-30, 30)
+            frame[glitch_y : glitch_y + glitch_height] = np.roll(
+                frame[glitch_y : glitch_y + glitch_height], shift, axis=1
+            )
+
+        text = f"NO SIGNAL ? {self.name} (#{self.cameraIndex})"
+        font = cv2.FONT_HERSHEY_SIMPLEX
+        font_scale = min(width, height) / 600
+        thickness = max(2, int(font_scale * 2))
+
+        text_size = cv2.getTextSize(text, font, font_scale, thickness)[0]
+        text_x = (width - text_size[0]) // 2
+        text_y = (height + text_size[1]) // 2
+
+        # Black outline
+        cv2.putText(
+            frame,
+            text,
+            (text_x, text_y),
+            font,
+            font_scale,
+            (0, 0, 0),
+            thickness + 3,
+            cv2.LINE_AA,
+        )
+
+        # White foreground
+        cv2.putText(
+            frame,
+            text,
+            (text_x, text_y),
+            font,
+            font_scale,
+            (255, 255, 255),
+            thickness,
+            cv2.LINE_AA,
+        )
+
+        return frame
 
     @classmethod
     @abstractmethod
@@ -172,12 +241,7 @@ def create(
 
     def setIndex(self, cameraIndex: CameraID) -> None:
         self.cameraIndex: CameraID = cameraIndex
-        s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
-
-        s.connect(("8.8.8.8", 80))
-        ip = s.getsockname()[0]
-        s.close()
-        self.stream = f"http://{ip}:{1181 + cameraIndex}/?action=stream/stream.mjpeg"
+        self.stream = ""
 
     @abstractmethod
     def grabFrame(self) -> Tuple[bool, Optional[Frame]]: ...
@@ -456,6 +520,7 @@ def grabFrame(self) -> Tuple[bool, Optional[np.ndarray]]:
                     return True, self._bufferPool[index]
             except queue.Empty:
                 pass
+
         return False, None
 
     def isConnected(self) -> bool:
@@ -642,6 +707,56 @@ def listToTransform3d(dataList: List[List[float]]) -> geometry.Transform3d:
         )
 
 
+class NoSignalCamera(SynapseCamera):
+    def __init__(self, name: str) -> None:
+        super().__init__(name=name)
+        self.resolution: Resolution = (640, 480)
+
+    @classmethod
+    def create(
+        cls, *_, path: Union[str, int] = 0, name: str = "", index: CameraID = -1
+    ) -> "NoSignalCamera":
+        inst = NoSignalCamera(name)
+        inst.setIndex(index)
+        return inst
+
+    def grabFrame(self) -> Tuple[bool, Optional[Frame]]:
+        # Always return a no-signal frame
+        return True, self.generateNoSignalFrame(self.resolution)
+
+    def isConnected(self) -> bool:
+        # Pretend the camera is never connected
+        return False
+
+    def close(self) -> None:
+        pass
+
+    def setProperty(self, prop: str, value: Union[int, float]) -> None:
+        # Ignore all property changes
+        pass
+
+    def getProperty(self, prop: str) -> Union[int, float, None]:
+        # No properties exist
+        return None
+
+    def setVideoMode(self, fps: int, width: int, height: int) -> None:
+        # Only store resolution for frame generation
+        self.resolution = (width, height)
+
+    def getResolution(self) -> Size:
+        return self.resolution
+
+    def getSupportedResolutions(self) -> List[Size]:
+        # Only support the current resolution
+        return [self.resolution]
+
+    def getPropertyMeta(self) -> Optional[PropertyMetaDict]:
+        return None
+
+    def getMaxFPS(self) -> float:
+        return 0.0
+
+
 def transform3dToList(transform: geometry.Transform3d) -> List[List[float]]:
     """
     Converts a Transform3d object into a 2D list containing position and rotation data.

synapse_core/src/synapse/core/camera_handler.py

@@ -13,13 +13,14 @@
 import cscore as cs
 import cv2
 import synapse.log as log
+from ntcore import NetworkTableInstance
 from synapse_net.nt_client import NtClient
 
 from ..callback import Callback
 from ..stypes import (CameraID, CameraName, CameraUID, Frame,
                       RecordingFilename, RecordingStatus, Resolution)
-from .camera_factory import (CameraConfig, CameraFactory, SynapseCamera,
-                             getCameraTableName)
+from .camera_factory import (CameraConfig, CameraFactory, NoSignalCamera,
+                             SynapseCamera, getCameraTableName)
 from .global_settings import GlobalSettings
 
 
@@ -54,6 +55,7 @@ def __init__(self) -> None:
         self.onAddCamera: Callback[CameraID, CameraName, SynapseCamera] = Callback()
         self.onRenameCamera: Callback[CameraID, CameraName] = Callback()
         self.cameraUIDs: List[CameraUID] = []
+        self.requestedCameraUIDs: Dict[CameraUID, CameraID] = {}
 
         self.cameraScanningThreadRunning: bool = True
         self.cameraScanningThread: threading.Thread
@@ -102,22 +104,25 @@ def createCameras(self) -> None:
             for info in cs.UsbCamera.enumerateUsbCameras()
         }
 
-        found: List[int] = []
-
         for cameraIndex, cameraConfig in self.cameraConfigBindings.items():
             if len(cameraConfig.id) > 0 and cameraConfig.id not in self.cameraUIDs:
                 info: Optional[cs.UsbCameraInfo] = self.usbCameraInfos.get(
                     cameraConfig.id, None
                 )
                 if info is not None:
-                    found.append(info.productId)
                     if not (self.addCamera(cameraIndex, cameraConfig, info.dev)):
                         continue
                     else:
                         self.cameraUIDs.append(cameraConfig.id)
                 else:
+                    self.requestedCameraUIDs[cameraConfig.id] = cameraIndex
+
+                    self.addCameraData(
+                        cameraIndex, cameraConfig, NoSignalCamera(cameraConfig.name)
+                    )
+
                     log.warn(
-                        f"No camera found for product id: {cameraConfig.id} (index: {cameraIndex}), camera will be skipped"
+                        f"No camera found for product id: {cameraConfig.id} (index: {cameraIndex}), camera will be skipped and added later"
                     )
                     continue
 
@@ -143,23 +148,31 @@ def scanCameras(self) -> None:
         found: List[int] = []
 
         for info in self.usbCameraInfos.values():
-            if info.productId not in found:
+            id = f"{info.name}_{info.productId}"
+            if info.productId not in found and id not in self.cameraUIDs:
                 found.append(info.productId)
                 newIndex = 0
-                if len(self.cameras.keys()) > 0:
-                    newIndex = max(self.cameras.keys()) + 1
+
+                print(self.requestedCameraUIDs)
+                if len(self.requestedCameraUIDs.keys()) > 0:
+                    if id in self.requestedCameraUIDs.keys():
+                        newIndex = self.requestedCameraUIDs.pop(id)
+                    else:
+                        m = max(self.requestedCameraUIDs.values())
+                        newIndex = m + 1
+
                 cameraIndex = newIndex
                 cameraConfig = CameraConfig(
                     name=info.name,
-                    id=f"{info.name}_{info.productId}",
+                    id=id,
                     defaultPipeline=0,
                     calibration={},
                     streamRes=self.DEFAULT_STREAM_SIZE,
                 )
 
                 if cameraConfig.id not in self.cameraUIDs:
                     log.log(
-                        f"Found non-registered camera: {info.name} (i={info.dev}), adding automatically"
+                        f"Found non-registered camera: {info.name} (i={cameraIndex}), adding automatically"
                     )
                     GlobalSettings.setCameraConfig(cameraIndex, cameraConfig)
                     if not (self.addCamera(cameraIndex, cameraConfig, info.dev)):
@@ -333,31 +346,53 @@ def addCamera(
             log.err(f"Failed to start camera capture: {e}")
             return False
 
-        MAX_RETRIES = 30
-        for attempt in range(MAX_RETRIES):
-            if camera.isConnected():
-                break
-            log.log(
-                f"Trying to open camera {camera.name} ({cameraConfig.id}), attempt {attempt + 1}"
-            )
-            time.sleep(1)
+        self.addCameraData(cameraIndex, cameraConfig, camera)
 
-        if camera.isConnected():
-            self.cameras[cameraIndex] = camera
-            self.streamOutputs[cameraIndex] = self.createStreamOutput(cameraIndex)
-            self.setRecordingStatus(cameraIndex, False)
+        return True
 
-            self.onAddCamera.call(cameraIndex, cameraConfig.name, camera)
+    def addCameraData(
+        self,
+        cameraIndex: CameraID,
+        cameraConfig: CameraConfig,
+        camera: SynapseCamera,
+    ):
+        if cameraIndex in self.cameras.keys():
+            prevCam = self.cameras.pop(cameraIndex)
+            prevCam.isRunning = False
+        self.cameras[cameraIndex] = camera
 
-            log.log(
-                f"Camera (name={cameraConfig.name}, id={cameraConfig.id}, id={cameraIndex}) added successfully."
-            )
-            return True
+        camera.cameraIndex = cameraIndex
 
-        log.err(
-            f"Failed to open camera {camera.name} ({cameraConfig.id}) after {MAX_RETRIES} retries."
+        self.streamOutputs[cameraIndex] = self.createStreamOutput(cameraIndex)
+
+        ret, frame = camera.grabFrame()
+
+        if frame is not None:
+            self.streamOutputs[cameraIndex].putFrame(
+                frame
+            )  # NOTE: stream only appears after a frame has been posted, probably
+
+        stream = (
+            NetworkTableInstance.getDefault()
+            .getTable("CameraPublisher")
+            .getSubTable(NtClient.NT_TABLE)
+            .getSubTable(cameraConfig.name)
+            .getStringArray("streams", [])[1]
+            .replace("mjpg:", "")
+        )
+
+        print(cameraConfig.name)
+        print(stream)
+
+        camera.stream = stream
+
+        self.setRecordingStatus(cameraIndex, False)
+
+        self.onAddCamera.call(cameraIndex, cameraConfig.name, camera)
+
+        log.log(
+            f"Camera (name={cameraConfig.name}, id={cameraConfig.id}, id={cameraIndex}) added successfully."
         )
-        return False
 
     def setCameraProps(
         self, settings: Dict[str, Any], camera: SynapseCamera

synapse_core/src/synapse/core/runtime_handler.py

@@ -10,7 +10,7 @@
 from asyncio import Queue
 from dataclasses import dataclass
 from pathlib import Path
-from typing import Any, Dict, Final, List, Optional, Tuple, TypeAlias
+from typing import Any, Dict, Final, Optional, Tuple, TypeAlias
 
 import cv2
 import numpy as np
@@ -87,7 +87,7 @@ def __init__(self, directory: Path):
         self.propPubs: Dict[Tuple[CameraID, str], Publisher] = {}
         self._lastFrameTime: dict[CameraID, float] = {}
         self.frameQueues: Dict[CameraID, Queue] = {}
-        self.cameraManagementThreads: List[threading.Thread] = []
+        self.cameraManagementThreads: Dict[CameraID, threading.Thread] = {}
 
         self.running = threading.Event()
         self.running.set()
@@ -130,10 +130,11 @@ def onAddCamera(cameraID: CameraID, name: str, camera: SynapseCamera):
                     {},
                 )
                 self.setPipelineByIndex(cameraID, self.pipelineBindings[cameraID])
+
             thread = threading.Thread(target=self.processCamera, args=(cameraID,))
             thread.daemon = True
             thread.start()
-            self.cameraManagementThreads.append(thread)
+            self.cameraManagementThreads[cameraID] = thread
 
         self.cameraHandler.onAddCamera.add(onAddCamera)
         self.cameraHandler.onAddCamera.add(self.pipelineHandler.onAddCamera)
@@ -525,16 +526,19 @@ def processCamera(self, cameraIndex: CameraID):
 
         log.log(f"Started {camera.name} loop (maxFPS={maxFps})")
 
-        while self.running.is_set():
+        while self.running.is_set() and camera.isRunning:
             loopStart = time.perf_counter()
 
-            ret, frame = camera.grabFrame()
-            if not ret or frame is None:
-                continue
+            if camera.isConnected():
+                ret, frame = camera.grabFrame()
+                if not ret or frame is None:
+                    continue
 
-            frame = self.fixtureFrame(cameraIndex, frame)
+                frame = self.fixtureFrame(cameraIndex, frame)
 
-            self._processAndPublishFrame(cameraIndex, frame)
+                self._processAndPublishFrame(cameraIndex, frame)
+            else:
+                self.cameraHandler.publishFrame(camera.generateNoSignalFrame(), camera)
 
             # Maintain camera FPS cap
             elapsed = time.perf_counter() - loopStart
@@ -840,7 +844,7 @@ def cleanup(self) -> None:
 
         self.cameraHandler.cleanup()
         self.running.clear()
-        for thread in self.cameraManagementThreads:
+        for thread in self.cameraManagementThreads.values():
             thread.join()
         if self.metricsThread:
             self.metricsThread.join()