259 lines
11 KiB
Python
259 lines
11 KiB
Python
"""Single-threaded finger acquisition API, independent of UART and the CLI.
|
|
|
|
Sensor IDs are 1-based, assigned once per successful add() and never reused
|
|
within a process, so a host that caches IDs stays correct across removals.
|
|
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 json
|
|
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
|
|
|
|
|
|
@dataclass(frozen=True)
|
|
class SensorInfo:
|
|
sensor_id: int
|
|
source: Source
|
|
kind: str # "camera" or "file"
|
|
config: str
|
|
calibration: str | None
|
|
|
|
|
|
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: dict[int, Source] = {}
|
|
self.configs: dict[int, str] = {}
|
|
self.runtimes: dict[int, object] = {}
|
|
self.calibration_paths: dict[int, str | None] = {}
|
|
self.verbose = verbose
|
|
self._next_id = 1
|
|
|
|
@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, cal_path: 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}")
|
|
if cal_path is not None:
|
|
if not str(cal_path).strip():
|
|
raise ValueError("empty calibration path")
|
|
calibration = Path(cal_path).expanduser().resolve()
|
|
if not calibration.is_file():
|
|
raise ValueError(f"calibration file not found: {calibration}")
|
|
cal_path = str(calibration)
|
|
from orisys.finger import FingerRuntime
|
|
|
|
config = self.default_config if config is None else config
|
|
runtime = None
|
|
try:
|
|
calibration_options = {"cal_path": cal_path} if cal_path is not None else {}
|
|
runtime = FingerRuntime.open(
|
|
config=config, video=source, verbose=self.verbose, **calibration_options,
|
|
)
|
|
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 = self._next_id
|
|
self._next_id += 1
|
|
self.sources[sensor_id] = source
|
|
self.configs[sensor_id] = config
|
|
self.runtimes[sensor_id] = runtime
|
|
self.calibration_paths[sensor_id] = cal_path
|
|
return sensor_id
|
|
|
|
def add_setup(self, path: str | Path) -> tuple[int, ...]:
|
|
"""Add an explicit JSON setup; rollback only this batch on failure.
|
|
|
|
Relative file paths are based on the setup file, not the process cwd.
|
|
Schema/identity validation precedes opening any camera.
|
|
"""
|
|
path = Path(path).expanduser().resolve()
|
|
document = json.loads(path.read_text(encoding="utf-8"))
|
|
if not isinstance(document, dict) or set(document) != {"sensors"}:
|
|
raise ValueError("setup must contain only a sensors list")
|
|
sensors = document["sensors"]
|
|
if not isinstance(sensors, list) or not 1 <= len(sensors) <= 4:
|
|
raise ValueError("setup must contain 1 to 4 sensors")
|
|
if len(self.runtimes) + len(sensors) > 4:
|
|
raise ValueError("maximum 4 sensors including those already added")
|
|
prepared = []
|
|
identities = {self._source_identity(source) for source in self.sources.values()}
|
|
for sensor in sensors:
|
|
if (not isinstance(sensor, dict) or "source" not in sensor
|
|
or set(sensor) - {"source", "config", "calibration"}):
|
|
raise ValueError("sensor fields: source, optional config and calibration")
|
|
if type(sensor["source"]) not in (int, str):
|
|
raise ValueError("source must be a camera index or path")
|
|
source = parse_source(str(sensor["source"]))
|
|
if isinstance(source, int) and source < 0:
|
|
raise ValueError("camera index must be non-negative")
|
|
if is_video_file(source):
|
|
source = str((path.parent / Path(source).expanduser()).resolve())
|
|
identity = self._source_identity(source)
|
|
if identity in identities:
|
|
raise ValueError("source already added or repeated in setup")
|
|
identities.add(identity)
|
|
config = sensor.get("config")
|
|
calibration = sensor.get("calibration")
|
|
for name, value in (("config", config), ("calibration", calibration)):
|
|
if value is not None and (not isinstance(value, str) or not value.strip()):
|
|
raise ValueError(f"{name} must be a non-empty string or null")
|
|
if config is not None and (Path(config).suffix.lower() == ".json" or "/" in config or "\\" in config):
|
|
config = str((path.parent / Path(config).expanduser()).resolve())
|
|
if calibration is not None:
|
|
calibration = str((path.parent / Path(calibration).expanduser()).resolve())
|
|
prepared.append({"source": source, "config": config, "cal_path": calibration})
|
|
added = []
|
|
try:
|
|
for sensor in prepared:
|
|
added.append(self.add(**sensor))
|
|
except BaseException:
|
|
for sensor_id in added:
|
|
try:
|
|
self.remove_sensor(sensor_id)
|
|
except Exception:
|
|
logger.exception("Setup rollback failed for sensor %s", sensor_id)
|
|
raise
|
|
return tuple(added)
|
|
|
|
def remove_sensor(self, sensor_id: int) -> None:
|
|
"""Close and forget one sensor. Its ID is never reused."""
|
|
if sensor_id not in self.runtimes:
|
|
raise ValueError(f"no sensor {sensor_id}")
|
|
runtime = self.runtimes.pop(sensor_id)
|
|
self.sources.pop(sensor_id, None)
|
|
self.configs.pop(sensor_id, None)
|
|
self.calibration_paths.pop(sensor_id, None)
|
|
runtime.close()
|
|
|
|
def list_sensors(self) -> tuple[SensorInfo, ...]:
|
|
"""Snapshot of currently registered sensors, in ID order."""
|
|
return tuple(
|
|
SensorInfo(
|
|
sensor_id=sid,
|
|
source=self.sources[sid],
|
|
kind="file" if is_video_file(self.sources[sid]) else "camera",
|
|
config=self.configs.get(sid, "-"),
|
|
calibration=self.calibration_paths.get(sid),
|
|
)
|
|
for sid in sorted(self.runtimes)
|
|
)
|
|
|
|
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 sorted(self.runtimes):
|
|
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.remove_sensor(sensor_id)
|
|
except Exception as exc:
|
|
logger.exception("Sensor %s cleanup failed", sensor_id)
|
|
first_error = first_error or exc
|
|
self.sources.clear()
|
|
self.configs.clear()
|
|
self.calibration_paths.clear()
|
|
if first_error is not None:
|
|
raise first_error
|