From 653ee60ef7f4cc57a3d9bd02881465a7502ee1c5 Mon Sep 17 00:00:00 2001 From: zhangyijian Date: Thu, 15 Oct 2026 13:42:29 +0800 Subject: [PATCH] =?UTF-8?q?=E4=B8=8A=E4=BC=A0=E6=96=87=E4=BB=B6=E8=87=B3?= =?UTF-8?q?=E3=80=8Cserial=E3=80=8D?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- serial/example_uart.py | 302 +++++++++++++++++++++++++++++++++-------- serial/finger_api.py | 71 +++++++--- 2 files changed, 294 insertions(+), 79 deletions(-) diff --git a/serial/example_uart.py b/serial/example_uart.py index 4ab79dd..5623c96 100644 --- a/serial/example_uart.py +++ b/serial/example_uart.py @@ -1,55 +1,100 @@ #!/usr/bin/env python3 -"""Headless UART finger service for 1-4 sensors; see ../docs/uart.md. +"""Headless UART finger service for 1-4 sensors. 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. +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. + +Commands (one line, case-insensitive, CR or LF terminated): + add SOURCE [CONFIG] [--calibration PATH] + load SETUP.json + remove SENSOR_ID + start | stop | read | quit | exit + list | status | help + +Replies: + OK ready + OK added + OK removed + OK loaded [ ...] + OK start | OK stop | OK quit + OK list + sensor,,,,, + OK status sensors= streaming=<0|1> loop_video=<0|1> + force,,,, + OK eof + ERR """ from __future__ import annotations import argparse +import difflib import logging import math import os import shlex import sys +import time from collections import deque os.environ.setdefault("OPENCV_VIDEOIO_MSMF_ENABLE_HW_TRANSFORMS", "0") -from finger_api import FingerSensorBank, parse_source +from finger_api import FingerSensorBank, is_video_file, parse_source logger = logging.getLogger(__name__) +_KNOWN_COMMANDS = ( + "add", "load", "remove", "start", "stop", "read", "quit", "exit", + "list", "status", "help", +) + +_HELP_LINES = ( + "add SOURCE [CONFIG] [--calibration PATH]", + "load SETUP.json", + "remove SENSOR_ID", + "start | stop | read | quit", + "list | status | help", +) + class UartFingerController: """Single-threaded command service; the caller owns serial and final cleanup.""" def __init__( self, ser, *, config="finger", loop_video=True, - show=False, verbose=False, + show=False, verbose=False, max_hz=0.0, prompt=False, ): self.ser = ser self.bank = FingerSensorBank(config=config, verbose=verbose) self.loop_video = loop_video self.show = show self.verbose = verbose + self.max_hz = max_hz + self.prompt = prompt self._streaming = False self._should_quit = False self._partial = bytearray() self._lines = deque() self._discard_line = False + # ------------------------------------------------------------------ io + def reply(self, line): # SDK exceptions can contain newlines; every reply must be one record. text = " ".join(str(line).splitlines()) + "\n" data = text.encode("utf-8") if self.ser.write(data) != len(data): raise OSError("incomplete UART write") + self.ser.flush() if self.verbose: print(f"<< {text.rstrip()}", file=sys.stderr, flush=True) + def _maybe_prompt(self): + if self.prompt and not self._streaming: + self.reply(">") + def close_runtime(self): self._streaming = False try: @@ -62,7 +107,10 @@ class UartFingerController: def _poll_command_line(self, *, blocking=True): if self._lines: - return self._lines.popleft() + line = self._lines.popleft() + if self.verbose: + print(f">> {line}", file=sys.stderr, flush=True) + return line waiting = self.ser.in_waiting if not waiting and not blocking: return None @@ -82,7 +130,14 @@ class UartFingerController: if len(self._partial) > 1024: self._partial.clear() self._discard_line = True - return self._lines.popleft() if self._lines else None + if self._lines: + line = self._lines.popleft() + if self.verbose: + print(f">> {line}", file=sys.stderr, flush=True) + return line + return None + + # ------------------------------------------------------------- streaming def _read_round(self): try: @@ -90,11 +145,13 @@ class UartFingerController: except EOFError as exc: self._streaming = False self.reply(f"OK eof {exc}") + self._maybe_prompt() return except Exception as exc: logger.exception("Acquisition round failed") self._streaming = False self.reply(f"ERR read {exc}") + self._maybe_prompt() return for reading in readings: self.reply( @@ -127,75 +184,180 @@ class UartFingerController: except Exception: logger.exception("Sensor %s display failed", reading.sensor_id) + # ------------------------------------------------------------- dispatch + + def _reply_hint(self, command: str): + close = difflib.get_close_matches(command, _KNOWN_COMMANDS, n=1) + suffix = f" (did you mean '{close[0]}'?)" if close else "" + self.reply(f"ERR unknown {command}{suffix}") + + def _cmd_add(self, parts): + if self._streaming: + self.reply("ERR add stop streaming first") + return + 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) + except (argparse.ArgumentError, SystemExit): + self.reply("ERR command expected add SOURCE [CONFIG] [--calibration PATH]") + return + try: + sensor_id = self.bank.add( + parse_source(args.source), config=args.config, cal_path=args.calibration, + ) + except FileNotFoundError: + self.reply(f"ERR add source not found: {args.source}") + except ValueError as exc: + self.reply(f"ERR add {exc}") + except Exception as exc: + logger.exception("Sensor addition failed") + self.reply( + f"ERR add failed for {args.source}: {type(exc).__name__}: {exc}" + ) + else: + src = self.bank.sources[sensor_id] + kind = "file" if is_video_file(src) else "camera" + self.reply(f"OK added {sensor_id} {kind} {src}") + + def _cmd_load(self, parts): + if len(parts) != 1: + self.reply("ERR command expected load SETUP.json") + return + if self._streaming: + self.reply("ERR load stop streaming first") + return + try: + sensor_ids = self.bank.add_setup(parts[0]) + except FileNotFoundError: + self.reply(f"ERR load setup not found: {parts[0]}") + except ValueError as exc: + self.reply(f"ERR load {exc}") + except Exception as exc: + logger.exception("Setup loading failed") + self.reply(f"ERR load failed: {type(exc).__name__}: {exc}") + else: + self.reply("OK loaded " + " ".join(map(str, sensor_ids))) + + def _cmd_remove(self, parts): + if self._streaming: + self.reply("ERR remove stop streaming first") + return + if len(parts) != 1 or not parts[0].isdigit(): + self.reply("ERR command expected remove SENSOR_ID") + return + sensor_id = int(parts[0]) + try: + self.bank.remove_sensor(sensor_id) + except ValueError as exc: + self.reply(f"ERR remove {exc}") + except Exception as exc: + logger.exception("Sensor removal failed") + self.reply(f"ERR remove failed: {type(exc).__name__}: {exc}") + else: + self.reply(f"OK removed {sensor_id}") + + def _cmd_list(self): + for info in self.bank.list_sensors(): + cal = info.calibration or "-" + self.reply( + f"sensor,{info.sensor_id},{info.kind},{info.source},{info.config},{cal}" + ) + self.reply(f"OK list {len(self.bank.sources)}") + + def _cmd_status(self): + self.reply( + f"OK status sensors={len(self.bank.sources)} " + f"streaming={int(self._streaming)} " + f"loop_video={int(self.loop_video)}" + ) + + def _cmd_help(self): + self.reply("OK help " + " | ".join(_HELP_LINES)) + + def _cmd_start(self, parts): + if parts: + self.reply("ERR command start takes no arguments") + return + if not self.bank.runtimes: + self.reply("ERR start no sensors added") + return + self._streaming = True + self.reply("OK start") + + def _cmd_stop(self, parts): + if parts: + self.reply("ERR command stop takes no arguments") + return + self._streaming = False + self.reply("OK stop") + self._maybe_prompt() + + def _cmd_read(self, parts): + if parts: + self.reply("ERR command read takes no arguments") + return + if not self.bank.runtimes: + self.reply("ERR read no sensors added") + return + self._read_round() + + def _cmd_quit(self, parts): + if parts: + self.reply("ERR command quit takes no arguments") + return + self._streaming = False + self._should_quit = True + self.reply("OK quit") + def _dispatch(self, line): try: parts = shlex.split(line) except ValueError as exc: self.reply(f"ERR command {exc}") + self._maybe_prompt() return True if not parts: + self._maybe_prompt() return True command = parts[0].lower() + args = parts[1:] + if command == "add": - 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(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}") + self._cmd_add(args) 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: - self.reply("ERR command start|stop|read|quit take no arguments") + self._cmd_load(args) + elif command == "remove": + self._cmd_remove(args) elif command == "start": - if not self.bank.runtimes: - self.reply("ERR start no sensors added") - else: - self._streaming = True - self.reply("OK start") + self._cmd_start(args) elif command == "stop": - self._streaming = False - self.reply("OK stop") + self._cmd_stop(args) elif command == "read": - self._read_round() - elif command == "quit": - self._streaming = False - self._should_quit = True - self.reply("OK quit") + self._cmd_read(args) + elif command in ("quit", "exit"): + self._cmd_quit(args) + elif command == "list": + self._cmd_list() + elif command == "status": + self._cmd_status() + elif command in ("help", "?"): + self._cmd_help() + else: + self._reply_hint(command) + + self._maybe_prompt() return not self._should_quit + # ---------------------------------------------------------------- serve + 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._maybe_prompt() self._streaming = start while not self._should_quit: for _ in range(16): @@ -205,7 +367,13 @@ class UartFingerController: if not self._dispatch(line): return if self._streaming: + t0 = time.monotonic() self._read_round() + if self.max_hz > 0 and self._streaming: + budget = 1.0 / self.max_hz - (time.monotonic() - t0) + if budget > 0: + time.sleep(budget) + def open_serial(port, baud, timeout, write_timeout=1.0): try: @@ -220,7 +388,9 @@ def open_serial(port, baud, timeout, write_timeout=1.0): def main(argv=None): - parser = argparse.ArgumentParser(description="Manual UART finger service: add, start, stop, read, quit") + parser = argparse.ArgumentParser( + description="Manual UART finger service: add, load, remove, start, stop, read, list, status, quit" + ) parser.add_argument("--port", default="/dev/ttyHS1") parser.add_argument("--baud", type=int, default=115200) parser.add_argument("--timeout", type=float, default=0.1, help="idle read timeout in seconds") @@ -231,6 +401,14 @@ def main(argv=None): 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( + "--max-hz", type=float, default=0.0, + help="cap force rounds per second while streaming (0 = unlimited)", + ) + parser.add_argument( + "--prompt", action="store_true", + help="emit a '>' line before each idle command wait (off by default)", + ) args = parser.parse_args(argv) if args.start and not args.setup: parser.error("--start requires --setup") @@ -238,9 +416,11 @@ def main(argv=None): 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 not math.isfinite(args.max_hz) or args.max_hz < 0: + parser.error("--max-hz must be finite and non-negative") controller = UartFingerController( None, config=args.config, loop_video=args.loop_video, - show=args.show, verbose=args.verbose, + show=args.show, verbose=args.verbose, max_hz=args.max_hz, prompt=args.prompt, ) ser = None result = 0 @@ -249,7 +429,11 @@ def main(argv=None): 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}; {len(controller.bank.runtimes)} sensors ready", file=sys.stderr, flush=True) + print( + f"UART service on {args.port} @ {args.baud}; " + f"{len(controller.bank.runtimes)} sensors ready", + file=sys.stderr, flush=True, + ) controller.serve(start=args.start) except KeyboardInterrupt: print("Interrupted.", file=sys.stderr) diff --git a/serial/finger_api.py b/serial/finger_api.py index 8a1ec63..6a4d987 100644 --- a/serial/finger_api.py +++ b/serial/finger_api.py @@ -1,8 +1,10 @@ """Single-threaded finger acquisition API, independent of UART and the CLI. -Sensor IDs are 1-based, assigned after each successful add(). 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. +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 @@ -40,20 +42,31 @@ class ForceReading: 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. + 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 = {} - self.configs = {} - self.runtimes = {} - self.calibration_paths = {} + 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): @@ -101,7 +114,8 @@ class FingerSensorBank: except Exception: logger.exception("New sensor cleanup failed") raise - sensor_id = len(self.runtimes) + 1 + sensor_id = self._next_id + self._next_id += 1 self.sources[sensor_id] = source self.configs[sensor_id] = config self.runtimes[sensor_id] = runtime @@ -157,12 +171,35 @@ class FingerSensorBank: except BaseException: for sensor_id in added: try: - self._close_one(sensor_id) + 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. @@ -171,7 +208,7 @@ class FingerSensorBank: if not self.runtimes: raise RuntimeError("no sensors added") readings = [] - for sensor_id in self.sources: + for sensor_id in sorted(self.runtimes): try: reading = self._read(sensor_id, loop_video=loop_video) except Exception as exc: @@ -206,22 +243,16 @@ 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._close_one(sensor_id) + 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