dragon-q6a/serial/finger_api.py

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

Sensor IDs are 1-based, fixed by source order. Calls and runtime ownership belong
to one thread; the caller must 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 1-4 independent runtimes; connect_all() 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, sources, *, configs=("finger",), verbose=False):
        sources = tuple(parse_source(str(source)) for source in sources)
        configs = tuple(configs)
        if not 1 <= len(sources) <= 4:
            raise ValueError("provide 1 to 4 sources")
        if len(configs) not in (1, len(sources)):
            raise ValueError("provide one config or one config per source")
        if any(isinstance(source, int) and source < 0 for source in sources):
            raise ValueError("camera index must be non-negative")
        if len(configs) == 1:
            configs = configs * len(sources)
        identities = []
        for source in sources:
            if isinstance(source, int):
                identity = f"camera:{source}"
            elif source.startswith("/dev/video") and source[len("/dev/video"):].isdigit():
                identity = f"camera:{int(source[len('/dev/video'):])}"
            else:
                identity = str(Path(source).resolve())
            identities.append(identity)
        if len(set(identities)) != len(identities):
            raise ValueError("each sensor must use a different source")
        self.sources = dict(enumerate(sources, 1))
        self.configs = dict(enumerate(configs, 1))
        self.runtimes = {}
        self.verbose = verbose

    def connect_all(self) -> None:
        """Prepare every source, rolling back all runtimes on any failure."""
        from orisys.finger import FingerRuntime

        if self.runtimes:
            raise RuntimeError("already connected; close before reconnecting")
        sensor_id = None
        try:
            for sensor_id, source in self.sources.items():
                if is_video_file(source) and not Path(source).is_file():
                    raise ValueError(f"file not found: {source}")
                runtime = FingerRuntime.open(
                    config=self.configs[sensor_id], video=source, verbose=self.verbose,
                )
                self.runtimes[sensor_id] = runtime
                ok, _ = runtime.warmup(color=False)
                if not ok:
                    raise RuntimeError("warmup could not read a frame")
        except Exception as exc:
            try:
                self.close()
            except Exception:
                logger.exception("Connection rollback failed")
            raise RuntimeError(f"sensor {sensor_id}: {exc}") from exc
        except BaseException:
            self.close()
            raise

    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 set(self.runtimes) != set(self.sources):
            raise RuntimeError("all configured sensors must be connected first")
        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
        if first_error is not None:
            raise first_error