dragon-q6a/serial/finger_api.py

"""Single-threaded finger acquisition API, independent of UART and the CLI.

Sensor IDs are 1-based, assigned after each successful add(). Calls and runtime
ownership belong to one thread; close the bank in a finally block. Values use the
SDK's FNORMAL/FSHEARX/FSHEARY axes and calibration units, not assumed Newtons.
"""

from __future__ import annotations

import logging
import math
from dataclasses import dataclass
from pathlib import Path

Source = int | str
logger = logging.getLogger(__name__)


def parse_source(token: str) -> Source:
    text = token.strip()
    if not text:
        raise ValueError("empty source")
    return int(text) if text.isdigit() or (text.startswith("-") and text[1:].isdigit()) else text


def is_video_file(source: Source) -> bool:
    from orisys.capture import is_live_camera

    return not is_live_camera(source) and not isinstance(source, int)


@dataclass(frozen=True)
class ForceReading:
    sensor_id: int
    normal: float
    shear_x: float
    shear_y: float
    units: tuple[str, str, str]
    image: object


class FingerSensorBank:
    """Own up to four independently added runtimes; add() precedes read_all().

    Processing/connection failures raise exceptions; they never become zero force.
    A shared config can be reused, or each source can have its own config.
    """

    def __init__(self, *, config="finger", verbose=False):
        self.default_config = config
        self.sources = {}
        self.configs = {}
        self.runtimes = {}
        self.verbose = verbose

    @staticmethod
    def _source_identity(source):
        if isinstance(source, int):
            return f"camera:{source}"
        if source.startswith("/dev/video") and source[len("/dev/video"):].isdigit():
            return f"camera:{int(source[len('/dev/video'):])}"
        return str(Path(source).resolve())

    def add(self, source: Source, *, config: str | None = None) -> int:
        """Open/prepare one source. Failure preserves existing sensors and IDs."""
        source = parse_source(str(source))
        if len(self.runtimes) >= 4:
            raise ValueError("maximum 4 sensors")
        if isinstance(source, int) and source < 0:
            raise ValueError("camera index must be non-negative")
        identity = self._source_identity(source)
        if any(self._source_identity(existing) == identity for existing in self.sources.values()):
            raise ValueError("source already added")
        if is_video_file(source) and not Path(source).is_file():
            raise ValueError(f"file not found: {source}")
        from orisys.finger import FingerRuntime

        config = self.default_config if config is None else config
        runtime = None
        try:
            runtime = FingerRuntime.open(config=config, video=source, verbose=self.verbose)
            ok, _ = runtime.warmup(color=False)
            if not ok:
                raise RuntimeError("warmup could not read a frame")
        except BaseException:
            if runtime is not None:
                try:
                    runtime.close()
                except Exception:
                    logger.exception("New sensor cleanup failed")
            raise
        sensor_id = len(self.runtimes) + 1
        self.sources[sensor_id] = source
        self.configs[sensor_id] = config
        self.runtimes[sensor_id] = runtime
        return sensor_id

    def read_all(self, *, loop_video=False) -> tuple[ForceReading, ...]:
        """Read a complete round. EOF/failure raises; no partial round is returned.

        The frames are read sequentially, not synchronized across sources.
        """
        if not self.runtimes:
            raise RuntimeError("no sensors added")
        readings = []
        for sensor_id in self.sources:
            try:
                reading = self._read(sensor_id, loop_video=loop_video)
            except Exception as exc:
                raise RuntimeError(f"sensor {sensor_id}: {exc}") from exc
            if reading is None:
                if is_video_file(self.sources[sensor_id]):
                    raise EOFError(f"sensor {sensor_id}: video ended")
                raise RuntimeError(f"sensor {sensor_id}: capture failed")
            readings.append(reading)
        return tuple(readings)

    def _read(self, sensor_id: int, *, loop_video=False) -> ForceReading | None:
        runtime = self.runtimes[sensor_id]
        ok, image = runtime.grab_and_process(color=False)
        if not ok and loop_video and is_video_file(self.sources[sensor_id]):
            import cv2

            # One rewind attempt per read prevents an empty file from spinning forever.
            if runtime.capture is not None and runtime.capture.set(cv2.CAP_PROP_POS_FRAMES, 0):
                runtime.tactile.reset()
                ok, image = runtime.grab_and_process(color=False)
        if not ok:
            return None
        tactile = runtime.tactile
        tactile.compute_deformation()
        tactile.compute_contact()
        forces = tuple(float(value) for value in tactile.read_info(
            tactile.info.FNORMAL, tactile.info.FSHEARX, tactile.info.FSHEARY,
        ))
        if not all(math.isfinite(value) for value in forces):
            raise RuntimeError("non-finite force measurement")
        units = getattr(tactile, "force_units", ("unknown", "unknown", "unknown"))
        return ForceReading(sensor_id, *forces, tuple(units), image)

    def close(self) -> None:
        first_error = None
        for sensor_id in list(self.runtimes):
            try:
                self.runtimes.pop(sensor_id).close()
            except Exception as exc:
                logger.exception("Sensor %s cleanup failed", sensor_id)
                first_error = first_error or exc
        self.sources.clear()
        self.configs.clear()
        if first_error is not None:
            raise first_error