修复 UART 命令处理并支持 1–4 路传感器
增加只读启动和可调用采集 API,保留单路数据协议。通过 28 项模拟回归测试、Ruff 和文档检查;实机限制见 docs/uart.md。
This commit is contained in:
@@ -0,0 +1,136 @@
|
||||
"""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. read() returns None at EOF/capture failure.
|
||||
|
||||
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=(0,), *, 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)
|
||||
self.sources = dict(enumerate(sources, 1))
|
||||
self.configs = dict(enumerate(configs, 1))
|
||||
self.runtimes = {}
|
||||
self.verbose = verbose
|
||||
|
||||
def validate_id(self, sensor_id: int) -> None:
|
||||
if sensor_id not in self.sources:
|
||||
raise ValueError(f"sensor ID must be 1..{len(self.sources)}")
|
||||
|
||||
def connect(self, sensor_id: int = 1, source: Source | None = None) -> Source:
|
||||
from orisys.finger import FingerRuntime
|
||||
|
||||
self.validate_id(sensor_id)
|
||||
source = self.sources[sensor_id] if source is None else parse_source(str(source))
|
||||
if isinstance(source, int) and source < 0:
|
||||
raise ValueError("camera index must be non-negative")
|
||||
if is_video_file(source) and not Path(source).is_file():
|
||||
raise ValueError(f"file not found: {source}")
|
||||
if source in (self.sources[i] for i in self.runtimes if i != sensor_id):
|
||||
raise ValueError(f"source already connected: {source}")
|
||||
self.disconnect(sensor_id)
|
||||
runtime = FingerRuntime.open(
|
||||
config=self.configs[sensor_id], video=source, verbose=self.verbose,
|
||||
)
|
||||
try:
|
||||
ok, _ = runtime.warmup(color=False)
|
||||
if not ok:
|
||||
raise RuntimeError("warmup could not read a frame")
|
||||
except BaseException:
|
||||
runtime.close()
|
||||
raise
|
||||
self.runtimes[sensor_id] = runtime
|
||||
self.sources[sensor_id] = source
|
||||
return source
|
||||
|
||||
def read(self, sensor_id: int = 1, *, loop_video=False) -> ForceReading | None:
|
||||
self.validate_id(sensor_id)
|
||||
if sensor_id not in self.runtimes:
|
||||
raise ValueError("not connected")
|
||||
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 disconnect(self, sensor_id: int = 1) -> None:
|
||||
self.validate_id(sensor_id)
|
||||
runtime = self.runtimes.pop(sensor_id, None)
|
||||
if runtime is not None:
|
||||
runtime.close()
|
||||
|
||||
def close(self) -> None:
|
||||
first_error = None
|
||||
for sensor_id in list(self.runtimes):
|
||||
try:
|
||||
self.disconnect(sensor_id)
|
||||
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
|
||||
Reference in New Issue
Block a user