上传文件至「serial」

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.
+explicitly by UART commands or a startup setup file. All force records include
-Importing this module opens no devices; acquisition is owned by finger_api.
+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 <n>
+  OK added <id> <kind> <source>
+  OK removed <id>
+  OK loaded <id> [<id> ...]
+  OK start | OK stop | OK quit
+  OK list <n>
+  sensor,<id>,<kind>,<source>,<config>,<calibration>
+  OK status sensors=<n> streaming=<0|1> loop_video=<0|1>
+  force,<id>,<fnormal>,<fshearx>,<fsheary>
+  OK eof <context>
+  ERR <context> <message>
 """
 
 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._cmd_add(args)
-                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}")
         elif command == "load":
-            if len(parts) != 2:
+            self._cmd_load(args)
-                self.reply("ERR command expected load SETUP.json")
+        elif command == "remove":
-            elif self._streaming:
+            self._cmd_remove(args)
-                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")
         elif command == "start":
-            if not self.bank.runtimes:
+            self._cmd_start(args)
-                self.reply("ERR start no sensors added")
-            else:
-                self._streaming = True
-                self.reply("OK start")
         elif command == "stop":
-            self._streaming = False
+            self._cmd_stop(args)
-            self.reply("OK stop")
         elif command == "read":
-            self._read_round()
+            self._cmd_read(args)
-        elif command == "quit":
+        elif command in ("quit", "exit"):
-            self._streaming = False
+            self._cmd_quit(args)
-            self._should_quit = True
+        elif command == "list":
-            self.reply("OK quit")
+            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)

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
+Sensor IDs are 1-based, assigned once per successful add() and never reused
-ownership belong to one thread; close the bank in a finally block. Values use the
+within a process, so a host that caches IDs stays correct across removals.
-SDK's FNORMAL/FSHEARX/FSHEARY axes and calibration units, not assumed Newtons.
+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.
+    Processing/connection failures raise exceptions; they never become zero
-    A shared config can be reused, or each source can have its own config.
+    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.sources: dict[int, Source] = {}
-        self.configs = {}
+        self.configs: dict[int, str] = {}
-        self.runtimes = {}
+        self.runtimes: dict[int, object] = {}
-        self.calibration_paths = {}
+        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