diff --git a/synapse_core/src/synapse/core/camera_factory.py b/synapse_core/src/synapse/core/camera_factory.py index dc95c649..dedb96c5 100644 --- a/synapse_core/src/synapse/core/camera_factory.py +++ b/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. diff --git a/synapse_core/src/synapse/core/camera_handler.py b/synapse_core/src/synapse/core/camera_handler.py index 2aa8adec..9c0908ac 100644 --- a/synapse_core/src/synapse/core/camera_handler.py +++ b/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,15 +148,23 @@ 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, @@ -159,7 +172,7 @@ def scanCameras(self) -> None: 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 diff --git a/synapse_core/src/synapse/core/runtime_handler.py b/synapse_core/src/synapse/core/runtime_handler.py index b69f7d23..d8edf554 100644 --- a/synapse_core/src/synapse/core/runtime_handler.py +++ b/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()