orisys-tech/dragon-q6a
3
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

上传文件至「serial」

Browse Source
This commit is contained in:
zhangyijian
2026-10-15 13:42:29 +08:00
parent a723e96dcc
commit 653ee60ef7
2 changed files with 294 additions and 79 deletions
Download Patch File Download Diff File Expand all files Collapse all files
serial/example_uart.py
+243 -59
View File
@@ -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 <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.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)
serial/finger_api.py
+51 -20
View File
@@ -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