统一多相机连接与 UART 数据协议
启动时完整连接所有配置相机，移除旧命令与单路数据格式。通过 22 项回归测试、真实 SDK 的 1–4 路录像验证、Ruff 和文档检查。
This commit is contained in:
ykcheungab
+66
-179
@@ -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. Single-sensor force
|
||||
records retain the original format; multi-sensor records add a 1-based ID.
|
||||
The login console and data UART must be separate ports. Every configured camera
|
||||
is prepared before UART output. All force records include a 1-based sensor ID.
|
||||
Importing this module opens no devices; acquisition is owned by finger_api.
|
||||
"""
|
||||
|
||||
@@ -12,14 +12,12 @@ import argparse
|
||||
import logging
|
||||
import math
|
||||
import os
|
||||
import shlex
|
||||
import sys
|
||||
from collections import deque
|
||||
from pathlib import Path
|
||||
|
||||
os.environ.setdefault("OPENCV_VIDEOIO_MSMF_ENABLE_HW_TRANSFORMS", "0")
|
||||
|
||||
from finger_api import FingerSensorBank, is_video_file, parse_source
|
||||
from finger_api import FingerSensorBank, parse_source
|
||||
|
||||
logger = logging.getLogger(__name__)
|
||||
|
||||
@@ -28,32 +26,21 @@ class UartFingerController:
|
||||
"""Single-threaded command service; the caller owns serial and final cleanup."""
|
||||
|
||||
def __init__(
|
||||
self, ser, *, config="finger", default_source=0, sources=None,
|
||||
configs=None, max_camera_index=10, loop_video=True, show=False,
|
||||
verbose=False, read_only=False, autostart=False,
|
||||
self, ser, *, sources, configs=("finger",), loop_video=True,
|
||||
show=False, verbose=False, read_only=False,
|
||||
):
|
||||
self.ser = ser
|
||||
self.bank = FingerSensorBank(
|
||||
sources if sources is not None else (default_source,),
|
||||
configs=configs if configs is not None else (config,), verbose=verbose,
|
||||
)
|
||||
self.max_camera_index = max_camera_index
|
||||
self.bank = FingerSensorBank(sources, configs=configs, verbose=verbose)
|
||||
self.loop_video = loop_video
|
||||
self.show = show
|
||||
self.verbose = verbose
|
||||
self.read_only = read_only
|
||||
self.autostart = autostart or read_only
|
||||
self._streaming = set()
|
||||
self._streaming = False
|
||||
self._should_quit = False
|
||||
self._last_search_source = None
|
||||
self._partial = bytearray()
|
||||
self._lines = deque()
|
||||
self._discard_line = False
|
||||
|
||||
@property
|
||||
def multi_sensor(self):
|
||||
return len(self.bank.sources) > 1
|
||||
|
||||
def reply(self, line):
|
||||
# SDK exceptions can contain newlines; every reply must be one record.
|
||||
text = " ".join(str(line).splitlines()) + "\n"
|
||||
@@ -64,7 +51,7 @@ class UartFingerController:
|
||||
print(f"<< {text.rstrip()}", file=sys.stderr, flush=True)
|
||||
|
||||
def close_runtime(self):
|
||||
self._streaming.clear()
|
||||
self._streaming = False
|
||||
try:
|
||||
self.bank.close()
|
||||
finally:
|
||||
@@ -99,112 +86,27 @@ class UartFingerController:
|
||||
self._discard_line = True
|
||||
return self._lines.popleft() if self._lines else None
|
||||
|
||||
def _targets(self, args, *, connected=False):
|
||||
if len(args) > 1:
|
||||
raise ValueError("expected [sensor-id|all]")
|
||||
if args and args[0].lower() != "all":
|
||||
sensor_id = int(args[0])
|
||||
self.bank.validate_id(sensor_id)
|
||||
return [sensor_id]
|
||||
return sorted(self.bank.runtimes if connected else self.bank.sources)
|
||||
|
||||
def _sensor_suffix(self, sensor_id):
|
||||
return f" {sensor_id}" if self.multi_sensor else ""
|
||||
|
||||
def cmd_search(self, args):
|
||||
if args:
|
||||
raise ValueError("search takes no arguments")
|
||||
if self._streaming:
|
||||
raise ValueError("stop streaming before camera search")
|
||||
def _read_round(self):
|
||||
try:
|
||||
from orisys.capture import list_cameras
|
||||
|
||||
cameras = list_cameras(max_index=self.max_camera_index)
|
||||
readings = self.bank.read_all(loop_video=self.loop_video)
|
||||
except EOFError as exc:
|
||||
self._streaming = False
|
||||
self.reply(f"OK eof {exc}")
|
||||
return
|
||||
except Exception as exc:
|
||||
logger.exception("Camera search failed")
|
||||
self.reply(f"ERR search {exc}")
|
||||
logger.exception("Acquisition round failed")
|
||||
self._streaming = False
|
||||
self.reply(f"ERR read {exc}")
|
||||
return
|
||||
self._last_search_source = None
|
||||
for cam in cameras:
|
||||
path = (cam.path or f"/dev/video{cam.index}").strip() or f"/dev/video{cam.index}"
|
||||
self.reply(f"camera,{cam.index},{path},{cam.width}x{cam.height}")
|
||||
if self._last_search_source is None:
|
||||
self._last_search_source = cam.index
|
||||
self.reply(f"OK search {len(cameras)}")
|
||||
|
||||
def cmd_connect(self, args):
|
||||
# Legacy `connect <source>` targets sensor 1. `connect <id> <source>`
|
||||
# selects a slot explicitly; quote source paths containing spaces.
|
||||
if args and args[0].lower() == "all":
|
||||
if len(args) != 1:
|
||||
raise ValueError("expected connect all")
|
||||
for sensor_id in self.bank.sources:
|
||||
self._connect_one(sensor_id)
|
||||
return
|
||||
sensor_id = 1
|
||||
if len(args) >= 2 and args[0].isdigit():
|
||||
sensor_id = int(args[0])
|
||||
args = args[1:]
|
||||
source = parse_source(" ".join(args)) if args else None
|
||||
if (source is None and not self.multi_sensor and self._last_search_source is not None
|
||||
and not is_video_file(self.bank.sources[1])):
|
||||
source = self._last_search_source
|
||||
self._connect_one(sensor_id, source)
|
||||
|
||||
def _connect_one(self, sensor_id, source=None):
|
||||
self.bank.validate_id(sensor_id)
|
||||
self._streaming.discard(sensor_id)
|
||||
try:
|
||||
source = self.bank.connect(sensor_id, source)
|
||||
except Exception as exc:
|
||||
logger.exception("Sensor %s connection failed", sensor_id)
|
||||
self.reply(f"ERR connect{self._sensor_suffix(sensor_id)} {exc}")
|
||||
return
|
||||
kind = "file" if is_video_file(source) else "camera"
|
||||
self.reply(f"OK connect{self._sensor_suffix(sensor_id)} {source} ({kind})")
|
||||
|
||||
def cmd_run(self, args):
|
||||
targets = self._targets(args, connected=True)
|
||||
if not targets or any(i not in self.bank.runtimes for i in targets):
|
||||
raise ValueError("not connected")
|
||||
if set(targets).issubset(self._streaming):
|
||||
raise ValueError("already running")
|
||||
self._streaming.update(targets)
|
||||
self.reply("OK run" + (f" {' '.join(map(str, targets))}" if self.multi_sensor else ""))
|
||||
|
||||
def cmd_stop(self, args):
|
||||
targets = self._targets(args)
|
||||
self._streaming.difference_update(targets)
|
||||
self.reply("OK stop" + (f" {' '.join(map(str, targets))}" if self.multi_sensor else ""))
|
||||
|
||||
def cmd_quit(self, args):
|
||||
if args:
|
||||
raise ValueError("quit takes no arguments")
|
||||
self._streaming.clear()
|
||||
self._should_quit = True
|
||||
self.reply("OK quit")
|
||||
|
||||
def _read_one(self, sensor_id, *, context):
|
||||
try:
|
||||
reading = self.bank.read(sensor_id, loop_video=self.loop_video)
|
||||
except Exception as exc:
|
||||
logger.exception("Sensor %s acquisition failed", sensor_id)
|
||||
self._streaming.discard(sensor_id)
|
||||
self.reply(f"ERR {context}{self._sensor_suffix(sensor_id)} {exc}")
|
||||
return
|
||||
if reading is None:
|
||||
self._streaming.discard(sensor_id)
|
||||
if is_video_file(self.bank.sources[sensor_id]):
|
||||
self.reply(f"OK eof{self._sensor_suffix(sensor_id)}")
|
||||
else:
|
||||
self.reply(f"ERR {context}{self._sensor_suffix(sensor_id)} grab failed")
|
||||
return
|
||||
sensor_prefix = f"{sensor_id}," if self.multi_sensor else ""
|
||||
self.reply(
|
||||
f"force,{sensor_prefix}{reading.normal:.6f},{reading.shear_x:.6f},{reading.shear_y:.6f}"
|
||||
)
|
||||
if self.show:
|
||||
self._show(reading)
|
||||
for reading in readings:
|
||||
self.reply(
|
||||
f"force,{reading.sensor_id},{reading.normal:.6f},"
|
||||
f"{reading.shear_x:.6f},{reading.shear_y:.6f}"
|
||||
)
|
||||
if self.show:
|
||||
self._show(reading)
|
||||
if self._should_quit:
|
||||
return
|
||||
|
||||
def _show(self, reading):
|
||||
import cv2
|
||||
@@ -219,7 +121,9 @@ class UartFingerController:
|
||||
cv2.imshow(f"UV Unrolled {reading.sensor_id}", arrows)
|
||||
key = cv2.waitKey(1) & 0xFF
|
||||
if key == ord("q"):
|
||||
self.cmd_quit([])
|
||||
self._streaming = False
|
||||
self._should_quit = True
|
||||
self.reply("OK quit")
|
||||
elif key == ord("r"):
|
||||
tactile.reset()
|
||||
except Exception:
|
||||
@@ -228,56 +132,42 @@ class UartFingerController:
|
||||
def _dispatch(self, line):
|
||||
if self.read_only:
|
||||
return True
|
||||
try:
|
||||
parts = shlex.split(line)
|
||||
except ValueError as exc:
|
||||
self.reply(f"ERR command {exc}")
|
||||
return True
|
||||
parts = line.split()
|
||||
if not parts:
|
||||
return True
|
||||
cmd, args = parts[0].lower(), parts[1:]
|
||||
try:
|
||||
if cmd == "search":
|
||||
self.cmd_search(args)
|
||||
elif cmd == "connect":
|
||||
self.cmd_connect(args)
|
||||
elif cmd == "run":
|
||||
self.cmd_run(args)
|
||||
elif cmd == "read":
|
||||
for sensor_id in self._targets(args):
|
||||
self._read_one(sensor_id, context="read")
|
||||
elif cmd == "stop":
|
||||
self.cmd_stop(args)
|
||||
elif cmd in ("quit", "exit"):
|
||||
self.cmd_quit(args)
|
||||
else:
|
||||
self.reply(f"ERR unknown {cmd}")
|
||||
except (ValueError, RuntimeError) as exc:
|
||||
self.reply(f"ERR {cmd} {exc}")
|
||||
command = parts[0].lower()
|
||||
if command not in {"start", "stop", "read", "quit"}:
|
||||
self.reply(f"ERR unknown {command}")
|
||||
elif len(parts) != 1:
|
||||
self.reply("ERR command start|stop|read|quit take no arguments")
|
||||
elif command == "start":
|
||||
self._streaming = True
|
||||
self.reply("OK start")
|
||||
elif command == "stop":
|
||||
self._streaming = False
|
||||
self.reply("OK stop")
|
||||
elif command == "read":
|
||||
self._read_round()
|
||||
elif command == "quit":
|
||||
self._streaming = False
|
||||
self._should_quit = True
|
||||
self.reply("OK quit")
|
||||
return not self._should_quit
|
||||
|
||||
def serve(self):
|
||||
self.reply("OK ready")
|
||||
if self.autostart:
|
||||
self.cmd_connect(["all"])
|
||||
if self.bank.runtimes:
|
||||
self.cmd_run([])
|
||||
else:
|
||||
raise RuntimeError("autostart could not connect any sensor")
|
||||
if set(self.bank.runtimes) != set(self.bank.sources):
|
||||
raise RuntimeError("connect all configured cameras before serving UART")
|
||||
self.reply(f"OK ready {len(self.bank.sources)}")
|
||||
self._streaming = True
|
||||
while not self._should_quit:
|
||||
# Bounded command work keeps a noisy host from starving acquisition.
|
||||
for _ in range(16):
|
||||
line = self._poll_command_line(blocking=not self._streaming)
|
||||
if line is None:
|
||||
break
|
||||
if not self._dispatch(line):
|
||||
return
|
||||
for sensor_id in sorted(self._streaming):
|
||||
self._read_one(sensor_id, context="run")
|
||||
if self._should_quit:
|
||||
return
|
||||
|
||||
|
||||
if self._streaming:
|
||||
self._read_round()
|
||||
def open_serial(port, baud, timeout, write_timeout=1.0):
|
||||
try:
|
||||
import serial
|
||||
@@ -297,38 +187,35 @@ 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", action="append", help="one shared config or repeat per sensor")
|
||||
parser.add_argument("--camera-id", type=int, default=0)
|
||||
parser.add_argument("--video", help="single-sensor default video file")
|
||||
parser.add_argument("--sources", nargs="+", help="1-4 camera indices, device paths or video files")
|
||||
parser.add_argument("--max-camera-index", type=int, default=10)
|
||||
parser.add_argument("--loop-video", dest="loop_video", action="store_true", default=True)
|
||||
parser.add_argument("--sources", required=True, nargs="+", help="1-4 camera indices, device paths or video files")
|
||||
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")
|
||||
parser.add_argument("--autostart", action="store_true", help="connect and run configured sources on startup")
|
||||
parser.add_argument("--read-only", action="store_true", help="autostart and ignore all UART commands")
|
||||
parser.add_argument("--read-only", action="store_true", help="ignore all UART commands; exit from the login console")
|
||||
args = parser.parse_args(argv)
|
||||
if args.baud <= 0 or args.max_camera_index < 0:
|
||||
parser.error("baud must be positive; max camera index must be non-negative")
|
||||
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)):
|
||||
parser.error("timeouts must be finite and positive")
|
||||
if args.sources and (args.video is not None or args.camera_id != 0):
|
||||
parser.error("--sources cannot be combined with --video or --camera-id")
|
||||
configs = args.config or ["finger"]
|
||||
try:
|
||||
sources = [parse_source(s) for s in args.sources] if args.sources else [
|
||||
str(Path(args.video).resolve()) if args.video else args.camera_id,
|
||||
]
|
||||
sources = [parse_source(s) for s in args.sources]
|
||||
controller = UartFingerController(
|
||||
None, sources=sources, configs=configs, max_camera_index=args.max_camera_index,
|
||||
None, sources=sources, configs=configs,
|
||||
loop_video=args.loop_video, show=args.show, verbose=args.verbose,
|
||||
autostart=args.autostart, read_only=args.read_only,
|
||||
read_only=args.read_only,
|
||||
)
|
||||
except ValueError as exc:
|
||||
parser.error(str(exc))
|
||||
ser = None
|
||||
result = 0
|
||||
try:
|
||||
for sensor_id, source in controller.bank.sources.items():
|
||||
print(
|
||||
f"Preparing sensor {sensor_id}: source={source} config={controller.bank.configs[sensor_id]}",
|
||||
file=sys.stderr, flush=True,
|
||||
)
|
||||
controller.bank.connect_all()
|
||||
ser = open_serial(args.port, args.baud, args.timeout, args.write_timeout)
|
||||
controller.ser = ser
|
||||
print(f"UART service on {args.port} @ {args.baud}; sources={sources}", file=sys.stderr, flush=True)
|
||||
|
||||
+57
-37
@@ -40,13 +40,13 @@ class ForceReading:
|
||||
|
||||
|
||||
class FingerSensorBank:
|
||||
"""Own 1–4 independent runtimes. read() returns None at EOF/capture failure.
|
||||
"""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=(0,), *, configs=("finger",), verbose=False):
|
||||
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:
|
||||
@@ -57,45 +57,71 @@ class FingerSensorBank:
|
||||
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 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:
|
||||
def connect_all(self) -> None:
|
||||
"""Prepare every source, rolling back all runtimes on any failure."""
|
||||
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,
|
||||
)
|
||||
if self.runtimes:
|
||||
raise RuntimeError("already connected; close before reconnecting")
|
||||
sensor_id = None
|
||||
try:
|
||||
ok, _ = runtime.warmup(color=False)
|
||||
if not ok:
|
||||
raise RuntimeError("warmup could not read a frame")
|
||||
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:
|
||||
runtime.close()
|
||||
self.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")
|
||||
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]):
|
||||
@@ -118,17 +144,11 @@ class FingerSensorBank:
|
||||
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)
|
||||
self.runtimes.pop(sensor_id).close()
|
||||
except Exception as exc:
|
||||
logger.exception("Sensor %s cleanup failed", sensor_id)
|
||||
first_error = first_error or exc
|
||||
|
||||
Reference in New Issue
Block a user