支持传感器独立校准文件与批量配置启动
保留逐台添加,并通过显式配置一次连接全组;失败时撤销本批新增连接。通过 35 项回归测试、真实 SDK 校准与 1–4 路验证、Ruff 和文档检查。
This commit is contained in:
+38
-10
@@ -1,8 +1,8 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Headless UART finger service for 1-4 sensors; see ../docs/uart.md.
|
||||
|
||||
The login console and data UART must be separate ports. Cameras are added
|
||||
manually via UART before starting output. All force records include a 1-based sensor ID.
|
||||
The login console and data UART must be separate ports. Cameras are selected
|
||||
explicitly by UART commands or a startup setup file. All force records include a sensor ID.
|
||||
Importing this module opens no devices; acquisition is owned by finger_api.
|
||||
"""
|
||||
|
||||
@@ -137,20 +137,40 @@ class UartFingerController:
|
||||
return True
|
||||
command = parts[0].lower()
|
||||
if command == "add":
|
||||
if len(parts) not in (2, 3):
|
||||
self.reply("ERR command expected add SOURCE [CONFIG]")
|
||||
elif self._streaming:
|
||||
if self._streaming:
|
||||
self.reply("ERR add stop streaming first")
|
||||
else:
|
||||
parser = argparse.ArgumentParser(prog="add", add_help=False, exit_on_error=False)
|
||||
parser.add_argument("source")
|
||||
parser.add_argument("config", nargs="?")
|
||||
parser.add_argument("--calibration")
|
||||
try:
|
||||
args = parser.parse_args(parts[1:])
|
||||
except (argparse.ArgumentError, SystemExit):
|
||||
self.reply("ERR command expected add SOURCE [CONFIG] [--calibration PATH]")
|
||||
return True
|
||||
try:
|
||||
sensor_id = self.bank.add(
|
||||
parse_source(parts[1]), config=parts[2] if len(parts) == 3 else None,
|
||||
parse_source(args.source), config=args.config, cal_path=args.calibration,
|
||||
)
|
||||
except Exception as exc:
|
||||
logger.exception("Sensor addition failed")
|
||||
self.reply(f"ERR add {exc}")
|
||||
else:
|
||||
self.reply(f"OK added {sensor_id}")
|
||||
elif command == "load":
|
||||
if len(parts) != 2:
|
||||
self.reply("ERR command expected load SETUP.json")
|
||||
elif self._streaming:
|
||||
self.reply("ERR load stop streaming first")
|
||||
else:
|
||||
try:
|
||||
sensor_ids = self.bank.add_setup(parts[1])
|
||||
except Exception as exc:
|
||||
logger.exception("Setup loading failed")
|
||||
self.reply(f"ERR load {exc}")
|
||||
else:
|
||||
self.reply("OK loaded " + " ".join(map(str, sensor_ids)))
|
||||
elif command not in {"start", "stop", "read", "quit"}:
|
||||
self.reply(f"ERR unknown {command}")
|
||||
elif len(parts) != 1:
|
||||
@@ -172,9 +192,11 @@ class UartFingerController:
|
||||
self.reply("OK quit")
|
||||
return not self._should_quit
|
||||
|
||||
def serve(self):
|
||||
def serve(self, *, start=False):
|
||||
if start and not self.bank.runtimes:
|
||||
raise RuntimeError("no sensors added")
|
||||
self.reply(f"OK ready {len(self.bank.sources)}")
|
||||
self._streaming = False
|
||||
self._streaming = start
|
||||
while not self._should_quit:
|
||||
for _ in range(16):
|
||||
line = self._poll_command_line(blocking=not self._streaming)
|
||||
@@ -204,10 +226,14 @@ def main(argv=None):
|
||||
parser.add_argument("--timeout", type=float, default=0.1, help="idle read timeout in seconds")
|
||||
parser.add_argument("--write-timeout", type=float, default=1.0)
|
||||
parser.add_argument("--config", "-c", default="finger", help="default config for add commands")
|
||||
parser.add_argument("--setup", help="JSON file listing sources, configs and calibrations to connect")
|
||||
parser.add_argument("--start", action="store_true", help="start output after loading --setup")
|
||||
parser.add_argument("--no-loop-video", dest="loop_video", action="store_false")
|
||||
parser.add_argument("--show", action="store_true", help="local desktop debug only")
|
||||
parser.add_argument("--verbose", action="store_true")
|
||||
args = parser.parse_args(argv)
|
||||
if args.start and not args.setup:
|
||||
parser.error("--start requires --setup")
|
||||
if args.baud <= 0:
|
||||
parser.error("baud must be positive")
|
||||
if any(not math.isfinite(t) or t <= 0 for t in (args.timeout, args.write_timeout)):
|
||||
@@ -219,10 +245,12 @@ def main(argv=None):
|
||||
ser = None
|
||||
result = 0
|
||||
try:
|
||||
if args.setup:
|
||||
controller.bank.add_setup(args.setup)
|
||||
ser = open_serial(args.port, args.baud, args.timeout, args.write_timeout)
|
||||
controller.ser = ser
|
||||
print(f"UART service on {args.port} @ {args.baud}; waiting for add SOURCE, then start", file=sys.stderr, flush=True)
|
||||
controller.serve()
|
||||
print(f"UART service on {args.port} @ {args.baud}; {len(controller.bank.runtimes)} sensors ready", file=sys.stderr, flush=True)
|
||||
controller.serve(start=args.start)
|
||||
except KeyboardInterrupt:
|
||||
print("Interrupted.", file=sys.stderr)
|
||||
except Exception:
|
||||
|
||||
+78
-3
@@ -7,6 +7,7 @@ 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
|
||||
@@ -51,6 +52,7 @@ class FingerSensorBank:
|
||||
self.sources = {}
|
||||
self.configs = {}
|
||||
self.runtimes = {}
|
||||
self.calibration_paths = {}
|
||||
self.verbose = verbose
|
||||
|
||||
@staticmethod
|
||||
@@ -61,7 +63,7 @@ class FingerSensorBank:
|
||||
return f"camera:{int(source[len('/dev/video'):])}"
|
||||
return str(Path(source).resolve())
|
||||
|
||||
def add(self, source: Source, *, config: str | None = None) -> int:
|
||||
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:
|
||||
@@ -73,12 +75,22 @@ class FingerSensorBank:
|
||||
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:
|
||||
runtime = FingerRuntime.open(config=config, video=source, verbose=self.verbose)
|
||||
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")
|
||||
@@ -93,8 +105,64 @@ class FingerSensorBank:
|
||||
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._close_one(sensor_id)
|
||||
except Exception:
|
||||
logger.exception("Setup rollback failed for sensor %s", sensor_id)
|
||||
raise
|
||||
return tuple(added)
|
||||
|
||||
def read_all(self, *, loop_video=False) -> tuple[ForceReading, ...]:
|
||||
"""Read a complete round. EOF/failure raises; no partial round is returned.
|
||||
|
||||
@@ -138,11 +206,18 @@ class FingerSensorBank:
|
||||
units = getattr(tactile, "force_units", ("unknown", "unknown", "unknown"))
|
||||
return ForceReading(sensor_id, *forces, tuple(units), image)
|
||||
|
||||
def _close_one(self, sensor_id):
|
||||
runtime = self.runtimes.pop(sensor_id)
|
||||
self.sources.pop(sensor_id)
|
||||
self.configs.pop(sensor_id)
|
||||
self.calibration_paths.pop(sensor_id)
|
||||
runtime.close()
|
||||
|
||||
def close(self) -> None:
|
||||
first_error = None
|
||||
for sensor_id in list(self.runtimes):
|
||||
try:
|
||||
self.runtimes.pop(sensor_id).close()
|
||||
self._close_one(sensor_id)
|
||||
except Exception as exc:
|
||||
logger.exception("Sensor %s cleanup failed", sensor_id)
|
||||
first_error = first_error or exc
|
||||
|
||||
@@ -0,0 +1,8 @@
|
||||
{
|
||||
"sensors": [
|
||||
{"source": "/dev/video0", "calibration": "../calibrations/finger/DD02-14.json"},
|
||||
{"source": "/dev/video2", "calibration": "../calibrations/finger/SENSOR-2.json"},
|
||||
{"source": "/dev/video4", "calibration": "../calibrations/finger/SENSOR-3.json"},
|
||||
{"source": "/dev/video6", "calibration": "../calibrations/finger/SENSOR-4.json"}
|
||||
]
|
||||
}
|
||||
Reference in New Issue
Block a user