修复 UART 命令处理并支持 1–4 路传感器
增加只读启动和可调用采集 API，保留单路数据协议。通过 28 项模拟回归测试、Ruff 和文档检查；实机限制见 docs/uart.md。
This commit is contained in:
ykcheungab
@@ -0,0 +1,4 @@
|
||||
# Dragon Q6A deployment helpers
|
||||
|
||||
See [UART setup, commands and API](docs/uart.md) for controlling or passively
|
||||
reading 1–4 sensors through a data UART separate from the TTL login console.
|
||||
|
||||
+141
@@ -0,0 +1,141 @@
|
||||
# UART finger service
|
||||
|
||||
`serial/example_uart.py` processes 1–4 finger cameras or recorded videos and
|
||||
publishes the SDK's normal and shear values over a separate data UART. Python
|
||||
3.10+, the board's installed `orisys` SDK and `pyserial` are required. Run inside
|
||||
the board environment prepared by `setup/sdk_setup.sh`; install `pyserial` there
|
||||
with `python -m pip install pyserial` if needed.
|
||||
|
||||
## Launch from the login console
|
||||
|
||||
The TTL login console and data UART must use **different device ports**. Start
|
||||
the script in the login shell; connect the data receiver to the port selected
|
||||
by `--port`. A shell/getty or another serial program must not own that data port.
|
||||
The default is `/dev/ttyHS1`, 115200 baud, 8N1, with no flow control. This service
|
||||
does not configure pin multiplexing or enable the board's UART hardware.
|
||||
|
||||
From the board's `dragon-q6a` checkout:
|
||||
|
||||
```bash
|
||||
# Host commands control connection and streaming (default: one camera).
|
||||
python serial/example_uart.py --port /dev/ttyHS1 --config finger
|
||||
|
||||
# Four configured sources; use 1, 2, 3 or 4 entries as required.
|
||||
python serial/example_uart.py --port /dev/ttyHS1 --sources 0 2 4 6
|
||||
|
||||
# A passive receiver needs only to read: connection and streaming start automatically.
|
||||
python serial/example_uart.py --port /dev/ttyHS1 --sources 0 2 --read-only
|
||||
|
||||
# Start streaming immediately, while still accepting UART commands.
|
||||
python serial/example_uart.py --port /dev/ttyHS1 --sources 0 2 --autostart
|
||||
```
|
||||
|
||||
Use actual camera indices/device paths discovered on the board; `0 2 4 6` is
|
||||
only an example. Slot IDs are fixed by `--sources` order, not camera index.
|
||||
Repeat `--config` once per source for different sensor configurations, or supply
|
||||
one shared configuration. Existing `--camera-id` and `--video` remain available
|
||||
for a single source. `--sources` replaces those options. `--show` requires a local
|
||||
GUI and is off by default. Logs go to the login console, replies to the data UART.
|
||||
|
||||
In `--read-only` mode, all incoming UART commands are ignored. End the process
|
||||
with Ctrl-C in the login shell. Startup still emits `OK`/`ERR` records; the receiver
|
||||
should select `force,` lines. Without `--autostart` or `--read-only`, a passive
|
||||
reader receives only `OK ready` until a host sends `connect` and `run`.
|
||||
|
||||
## Command protocol
|
||||
|
||||
Commands are UTF-8, case-insensitive, terminated by LF, CR or CRLF. Incomplete
|
||||
lines remain buffered across serial timeouts. Lines exceeding 1024 bytes are
|
||||
discarded through their terminator and produce `ERR command ...` in control mode.
|
||||
Quote file paths containing spaces. Linux device paths are recommended on Q6A.
|
||||
|
||||
| Command | Behavior |
|
||||
|---|---|
|
||||
| `search` | List cameras, then `OK search N`; use before connecting. Rejected while streaming. |
|
||||
| `connect` | Connect slot 1 using its configured source; in single-source mode, prefer the latest search result for a camera. |
|
||||
| `connect SOURCE` | Legacy form: connect slot 1 to this camera index, device path or file. |
|
||||
| `connect ID SOURCE` | Connect/replace one slot; stop streaming that slot. Other slots keep their connections. |
|
||||
| `connect all` | Connect all configured slots; report each success/failure independently. |
|
||||
| `run [ID\|all]` | Stream the selected slot, or all connected slots when omitted. Requires a connection. |
|
||||
| `read [ID\|all]` | Publish one newly processed measurement per selected slot; defaults to all configured slots. Does not start streaming. |
|
||||
| `stop [ID\|all]` | Stop selected streaming, or all when omitted; keep cameras connected. |
|
||||
| `quit` / `exit` | Exit and release all camera and UART resources. |
|
||||
|
||||
`connect 2` means **camera index 2 on slot 1**, preserving the old protocol.
|
||||
To connect slot 2, send `connect 2 /dev/video2` or use `connect all`.
|
||||
There are no motor-control commands; these commands control acquisition only.
|
||||
|
||||
Single-source measurements retain the original format:
|
||||
|
||||
```text
|
||||
force,1.250000,-2.500000,3.750000
|
||||
```
|
||||
|
||||
When configured with multiple sources, measurements include the slot ID even if
|
||||
only one source connected successfully:
|
||||
|
||||
```text
|
||||
OK connect 2 /dev/video2 (camera)
|
||||
OK run 1 2
|
||||
force,1,1.250000,-2.500000,3.750000
|
||||
force,2,0.500000,0.100000,-0.200000
|
||||
ERR run 2 processing failed
|
||||
OK eof 1
|
||||
```
|
||||
|
||||
Force fields are `FNORMAL`, `FSHEARX`, `FSHEARY` in SDK axes. They may be raw
|
||||
or calibrated; **do not assume Newtons**. The Python API exposes SDK units per
|
||||
reading, using `unknown` if an older SDK does not expose them. CSV preserves
|
||||
the original fields and carries no units, timestamps or cross-sensor synchronization.
|
||||
|
||||
Each processing failure or EOF stops only the affected slot. Processing exceptions
|
||||
produce `ERR`, never fabricated zero force. `--no-loop-video` stops files at EOF;
|
||||
the default tries one rewind, resets the tracker and retries one frame. Failed
|
||||
preparation releases the newly opened runtime; replacing a connection releases
|
||||
the old one before opening the replacement. A failed replacement requires retrying
|
||||
`connect`. Startup continues with any successfully connected slots and fails if
|
||||
none connect. Reconnect and run explicitly after a camera failure.
|
||||
|
||||
## Callable acquisition API
|
||||
|
||||
`serial/finger_api.py` owns acquisition and runtime cleanup; importing it opens
|
||||
no UART or camera. Place that directory on the caller's module search path:
|
||||
|
||||
```python
|
||||
from finger_api import FingerSensorBank
|
||||
|
||||
bank = FingerSensorBank([0, 2], configs=["finger"])
|
||||
try:
|
||||
bank.connect(1)
|
||||
bank.connect(2)
|
||||
reading = bank.read(1)
|
||||
if reading is not None:
|
||||
print(reading.normal, reading.shear_x, reading.shear_y, reading.units)
|
||||
finally:
|
||||
bank.close()
|
||||
```
|
||||
|
||||
The caller owns one thread and must close the bank. `read()` returns `None` on
|
||||
capture failure/EOF and raises on processing failure. It returns an image and
|
||||
three scalar forces from the same processed frame. One file or camera is read
|
||||
at a time; the API introduces no workers beyond those owned by the SDK.
|
||||
|
||||
## Verification and limits
|
||||
|
||||
Hardware-free regression tests cover command fragmentation, CR/LF, legacy output,
|
||||
1–4 slot identities, selective control, failure isolation, read-only startup and
|
||||
cleanup. They use in-memory serial and runtime fakes, not physical devices:
|
||||
|
||||
```bash
|
||||
python -m unittest discover -s tests -v
|
||||
```
|
||||
|
||||
Q6A UART wiring, installed ARM SDK compatibility, camera bandwidth, sustained
|
||||
frame rates and latency remain unverified. The shipped ARM wheel is compiled
|
||||
`orisys 0.5.0`; local tests do not execute it. SDK capture and processing remain
|
||||
synchronous from this service's perspective. Commands are checked before each
|
||||
round; a slow grab, processing call, connection or camera scan can delay them.
|
||||
Serial writes have a finite timeout (default 1 second), and transport failure
|
||||
terminates the service with cleanup. At 115200 baud/8N1, the data budget is
|
||||
11,520 bytes/s, shared by all measurements and replies. Measure actual frame
|
||||
rates and stop latency on the board before choosing a higher baud rate.
|
||||
+265
-372
@@ -1,460 +1,353 @@
|
||||
#!/usr/bin/env python3
|
||||
"""UART finger controller for Radxa / Dragon Q6A (sensor-side receiver).
|
||||
"""Headless UART finger service for 1-4 sensors; see ../docs/uart.md.
|
||||
|
||||
Combines ORY-FINGER processing (same path as ``examples/finger_pipeline.py``)
|
||||
with a line-based UART command interface. A host (or ``uart_send.py``-style
|
||||
transmitter) sends commands; this script replies and streams forces.
|
||||
|
||||
Dependencies (beyond orisys):
|
||||
pip install pyserial
|
||||
|
||||
Default UART (Dragon Q6A):
|
||||
port=/dev/ttyHS1 baud=115200 8N1
|
||||
|
||||
Commands (one line, case-insensitive):
|
||||
search List cameras (/dev/video*) via orisys.capture.list_cameras
|
||||
connect [source] Open FingerRuntime (camera index, /dev/videoN, or video file)
|
||||
run Stream FNORMAL / FSHEARX / FSHEARY until stop (or EOF)
|
||||
stop Stop force streaming; keep source connected
|
||||
quit | exit Close runtime and serial, then exit
|
||||
|
||||
Replies:
|
||||
camera,<index>,<path>,<WxH>
|
||||
OK search <N>
|
||||
OK connect <source>
|
||||
OK run
|
||||
force,<fnormal>,<fshearx>,<fsheary>
|
||||
OK eof Video file ended (when --no-loop-video)
|
||||
OK stop
|
||||
OK quit
|
||||
ERR <context> <message>
|
||||
|
||||
Examples:
|
||||
# Live camera over UART
|
||||
python scripts/example_uart.py --port /dev/ttyHS1 --config finger
|
||||
|
||||
# Offline debug from a recorded file (no camera needed)
|
||||
python scripts/example_uart.py --port /dev/ttyHS1 --video /path/to/clip.mp4
|
||||
# then UART: connect (uses --video) or connect /path/to/clip.mp4
|
||||
# then UART: run
|
||||
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.
|
||||
Importing this module opens no devices; acquisition is owned by finger_api.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
import logging
|
||||
import math
|
||||
import os
|
||||
import shlex
|
||||
import sys
|
||||
from collections import deque
|
||||
from pathlib import Path
|
||||
from typing import Optional, Union
|
||||
|
||||
os.environ.setdefault("OPENCV_VIDEOIO_MSMF_ENABLE_HW_TRANSFORMS", "0")
|
||||
|
||||
try:
|
||||
import serial
|
||||
except ImportError as exc: # pragma: no cover
|
||||
raise SystemExit(
|
||||
"pyserial is required for this script.\n"
|
||||
" pip install pyserial\n"
|
||||
f"Import error: {exc}"
|
||||
) from exc
|
||||
from finger_api import FingerSensorBank, is_video_file, parse_source
|
||||
|
||||
import cv2
|
||||
from orisys.capture import is_live_camera, list_cameras
|
||||
from orisys.finger import FingerRuntime
|
||||
|
||||
Source = Union[int, str]
|
||||
|
||||
|
||||
def _parse_source(token: str) -> Optional[Source]:
|
||||
"""Accept camera index int, numeric string, V4L path, or filesystem path."""
|
||||
text = str(token).strip().strip('"').strip("'")
|
||||
if not text:
|
||||
return None
|
||||
if text.isdigit() or (text.startswith("-") and text[1:].isdigit()):
|
||||
return int(text)
|
||||
return text
|
||||
|
||||
|
||||
def _is_video_file(source: Source) -> bool:
|
||||
"""True when source is a filesystem video path (not a live camera)."""
|
||||
if is_live_camera(source):
|
||||
return False
|
||||
if isinstance(source, int):
|
||||
return False
|
||||
path = Path(str(source))
|
||||
return path.is_file() or path.suffix.lower() in {
|
||||
".mp4",
|
||||
".avi",
|
||||
".mkv",
|
||||
".mov",
|
||||
".m4v",
|
||||
".wmv",
|
||||
".webm",
|
||||
}
|
||||
logger = logging.getLogger(__name__)
|
||||
|
||||
|
||||
class UartFingerController:
|
||||
"""Serial command dispatcher around a single FingerRuntime."""
|
||||
"""Single-threaded command service; the caller owns serial and final cleanup."""
|
||||
|
||||
def __init__(
|
||||
self,
|
||||
ser: serial.Serial,
|
||||
*,
|
||||
config: str = "finger",
|
||||
default_source: Source = 0,
|
||||
max_camera_index: int = 10,
|
||||
loop_video: bool = True,
|
||||
show: bool = False,
|
||||
verbose: bool = False,
|
||||
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 = ser
|
||||
self.config = config
|
||||
self.default_source = default_source
|
||||
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.loop_video = loop_video
|
||||
self.show = show
|
||||
self.verbose = verbose
|
||||
|
||||
self.runtime: Optional[FingerRuntime] = None
|
||||
self._connected_source: Optional[Source] = None
|
||||
self._last_search_source: Optional[Source] = None
|
||||
self._streaming = False
|
||||
self.read_only = read_only
|
||||
self.autostart = autostart or read_only
|
||||
self._streaming = set()
|
||||
self._should_quit = False
|
||||
self._last_search_source = None
|
||||
self._partial = bytearray()
|
||||
self._lines = deque()
|
||||
self._discard_line = False
|
||||
|
||||
def reply(self, line: str) -> None:
|
||||
text = line.rstrip("\r\n") + "\n"
|
||||
self.ser.write(text.encode("utf-8"))
|
||||
self.ser.flush()
|
||||
@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"
|
||||
data = text.encode("utf-8")
|
||||
if self.ser.write(data) != len(data):
|
||||
raise OSError("incomplete UART write")
|
||||
if self.verbose:
|
||||
print(f"<< {text.rstrip()}", flush=True)
|
||||
print(f"<< {text.rstrip()}", file=sys.stderr, flush=True)
|
||||
|
||||
def close_runtime(self) -> None:
|
||||
self._streaming = False
|
||||
if self.runtime is not None:
|
||||
def close_runtime(self):
|
||||
self._streaming.clear()
|
||||
try:
|
||||
self.runtime.close()
|
||||
except Exception: # noqa: BLE001
|
||||
pass
|
||||
self.runtime = None
|
||||
self._connected_source = None
|
||||
self.bank.close()
|
||||
finally:
|
||||
if self.show:
|
||||
try:
|
||||
import cv2
|
||||
|
||||
cv2.destroyAllWindows()
|
||||
except Exception: # noqa: BLE001
|
||||
pass
|
||||
|
||||
def _resolve_connect_source(self, args: list[str]) -> Source:
|
||||
def _poll_command_line(self, *, blocking=True):
|
||||
if self._lines:
|
||||
return self._lines.popleft()
|
||||
waiting = self.ser.in_waiting
|
||||
if not waiting and not blocking:
|
||||
return None
|
||||
# Never readline(): a timeout may return only half of a command.
|
||||
raw = self.ser.read(min(waiting, 4096) if waiting else 1)
|
||||
if self.read_only:
|
||||
return None
|
||||
for byte in raw:
|
||||
if byte in (10, 13):
|
||||
if self._discard_line:
|
||||
self._discard_line = False
|
||||
self._partial.clear()
|
||||
self.reply("ERR command line too long (max 1024 bytes)")
|
||||
elif self._partial:
|
||||
self._lines.append(self._partial.decode("utf-8", errors="replace").strip())
|
||||
self._partial.clear()
|
||||
elif not self._discard_line:
|
||||
self._partial.append(byte)
|
||||
if len(self._partial) > 1024:
|
||||
self._partial.clear()
|
||||
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:
|
||||
# Join remaining tokens so unquoted paths with spaces still work.
|
||||
joined = " ".join(args).strip()
|
||||
parsed = _parse_source(joined)
|
||||
if parsed is None:
|
||||
raise ValueError("empty connect source")
|
||||
return parsed
|
||||
# Prefer --video default over last search so offline debug stays on file.
|
||||
if isinstance(self.default_source, str) and not is_live_camera(self.default_source):
|
||||
return self.default_source
|
||||
if self._last_search_source is not None:
|
||||
return self._last_search_source
|
||||
return self.default_source
|
||||
|
||||
def cmd_search(self, _args: list[str]) -> None:
|
||||
raise ValueError("search takes no arguments")
|
||||
if self._streaming:
|
||||
raise ValueError("stop streaming before camera search")
|
||||
try:
|
||||
from orisys.capture import list_cameras
|
||||
|
||||
cameras = list_cameras(max_index=self.max_camera_index)
|
||||
except Exception as exc: # noqa: BLE001
|
||||
except Exception as exc:
|
||||
logger.exception("Camera search failed")
|
||||
self.reply(f"ERR search {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}"
|
||||
size = f"{cam.width}x{cam.height}"
|
||||
self.reply(f"camera,{cam.index},{path},{size}")
|
||||
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: list[str]) -> None:
|
||||
try:
|
||||
source = self._resolve_connect_source(args)
|
||||
except ValueError as exc:
|
||||
self.reply(f"ERR connect {exc}")
|
||||
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)
|
||||
|
||||
if isinstance(source, str) and not is_live_camera(source):
|
||||
path = Path(source)
|
||||
if not path.is_file():
|
||||
self.reply(f"ERR connect file not found: {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})")
|
||||
|
||||
self._streaming = False
|
||||
self.close_runtime()
|
||||
try:
|
||||
runtime = FingerRuntime.open(
|
||||
config=self.config,
|
||||
video=source,
|
||||
verbose=self.verbose,
|
||||
)
|
||||
runtime.warmup(color=False)
|
||||
except Exception as exc: # noqa: BLE001
|
||||
self.reply(f"ERR connect {exc}")
|
||||
return
|
||||
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 ""))
|
||||
|
||||
self.runtime = runtime
|
||||
self._connected_source = source
|
||||
kind = "file" if _is_video_file(source) else "camera"
|
||||
self.reply(f"OK connect {source} ({kind})")
|
||||
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 _rewind_video(self) -> bool:
|
||||
"""Seek connected file source back to frame 0. Return True on success."""
|
||||
if self.runtime is None or self._connected_source is None:
|
||||
return False
|
||||
if not _is_video_file(self._connected_source):
|
||||
return False
|
||||
cap = self.runtime.capture
|
||||
if cap is None:
|
||||
return False
|
||||
try:
|
||||
return bool(cap.set(cv2.CAP_PROP_POS_FRAMES, 0))
|
||||
except Exception: # noqa: BLE001
|
||||
return False
|
||||
|
||||
def _poll_command_line(self, *, blocking: bool = True) -> Optional[str]:
|
||||
"""Read one command line if available; return None on timeout/empty."""
|
||||
if not blocking and self.ser.in_waiting <= 0:
|
||||
return None
|
||||
raw = self.ser.readline()
|
||||
if not raw:
|
||||
return None
|
||||
try:
|
||||
line = raw.decode("utf-8", errors="replace").strip()
|
||||
except Exception: # noqa: BLE001
|
||||
return None
|
||||
return line or None
|
||||
|
||||
def cmd_run(self, _args: list[str]) -> None:
|
||||
if self.runtime is None:
|
||||
self.reply("ERR run not connected")
|
||||
return
|
||||
|
||||
self._streaming = True
|
||||
self.reply("OK run")
|
||||
tactile = self.runtime.tactile
|
||||
is_file = self._connected_source is not None and _is_video_file(self._connected_source)
|
||||
|
||||
while self._streaming and not self._should_quit:
|
||||
ok, unrolled = self.runtime.grab_and_process(color=False)
|
||||
if not ok:
|
||||
if is_file and self.loop_video and self._rewind_video():
|
||||
if self.verbose:
|
||||
print("video EOF: looping", flush=True)
|
||||
continue
|
||||
if is_file:
|
||||
self.reply("OK eof")
|
||||
else:
|
||||
self.reply("ERR run grab failed")
|
||||
self._streaming = False
|
||||
break
|
||||
|
||||
tactile.compute_deformation()
|
||||
tactile.compute_contact()
|
||||
fnormal, fshearx, fsheary = tactile.read_info(
|
||||
tactile.info.FNORMAL,
|
||||
tactile.info.FSHEARX,
|
||||
tactile.info.FSHEARY,
|
||||
)
|
||||
self.reply(f"force,{float(fnormal):.6f},{float(fshearx):.6f},{float(fsheary):.6f}")
|
||||
|
||||
if self.show:
|
||||
try:
|
||||
import orisys
|
||||
|
||||
arrows = orisys.util.draw_arrows(
|
||||
unrolled,
|
||||
tactile.flow,
|
||||
threshold=0.4,
|
||||
grid_spacing=10,
|
||||
arrow_scale=1.0,
|
||||
valid_mask=tactile.valid_mask,
|
||||
)
|
||||
cv2.imshow("UV Unrolled", arrows)
|
||||
key = cv2.waitKey(1) & 0xFF
|
||||
if key == ord("q"):
|
||||
self._should_quit = True
|
||||
break
|
||||
if key == ord("r"):
|
||||
tactile.reset()
|
||||
except Exception as exc: # noqa: BLE001
|
||||
if self.verbose:
|
||||
print(f"show error: {exc}", file=sys.stderr)
|
||||
|
||||
# Non-blocking command poll so force streaming is not delayed by UART timeout.
|
||||
pending = self._poll_command_line(blocking=False)
|
||||
if pending is not None:
|
||||
if self.verbose:
|
||||
print(f">> {pending}", flush=True)
|
||||
if not self._dispatch(pending, allow_run=False):
|
||||
break
|
||||
|
||||
def cmd_stop(self, _args: list[str]) -> None:
|
||||
self._streaming = False
|
||||
self.reply("OK stop")
|
||||
|
||||
def cmd_quit(self, _args: list[str]) -> None:
|
||||
self._streaming = False
|
||||
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 _dispatch(self, line: str, *, allow_run: bool = True) -> bool:
|
||||
"""Dispatch one command line. Return False if the main loop should exit."""
|
||||
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)
|
||||
|
||||
def _show(self, reading):
|
||||
import cv2
|
||||
import orisys
|
||||
|
||||
tactile = self.bank.runtimes[reading.sensor_id].tactile
|
||||
try:
|
||||
arrows = orisys.util.draw_arrows(
|
||||
reading.image, tactile.flow, threshold=0.4, grid_spacing=10,
|
||||
arrow_scale=1.0, valid_mask=tactile.valid_mask,
|
||||
)
|
||||
cv2.imshow(f"UV Unrolled {reading.sensor_id}", arrows)
|
||||
key = cv2.waitKey(1) & 0xFF
|
||||
if key == ord("q"):
|
||||
self.cmd_quit([])
|
||||
elif key == ord("r"):
|
||||
tactile.reset()
|
||||
except Exception:
|
||||
logger.exception("Sensor %s display failed", reading.sensor_id)
|
||||
|
||||
def _dispatch(self, line):
|
||||
if self.read_only:
|
||||
return True
|
||||
try:
|
||||
parts = shlex.split(line)
|
||||
except ValueError:
|
||||
parts = line.split()
|
||||
except ValueError as exc:
|
||||
self.reply(f"ERR command {exc}")
|
||||
return True
|
||||
if not parts:
|
||||
return True
|
||||
cmd = parts[0].lower()
|
||||
args = parts[1:]
|
||||
|
||||
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":
|
||||
if not allow_run:
|
||||
self.reply("ERR run already running")
|
||||
else:
|
||||
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)
|
||||
return False
|
||||
else:
|
||||
self.reply(f"ERR unknown {cmd}")
|
||||
except (ValueError, RuntimeError) as exc:
|
||||
self.reply(f"ERR {cmd} {exc}")
|
||||
return not self._should_quit
|
||||
|
||||
def serve(self) -> None:
|
||||
def serve(self):
|
||||
self.reply("OK ready")
|
||||
while not self._should_quit:
|
||||
line = self._poll_command_line()
|
||||
if line is None:
|
||||
continue
|
||||
if self.verbose:
|
||||
print(f">> {line}", flush=True)
|
||||
if not self._dispatch(line):
|
||||
break
|
||||
|
||||
|
||||
def open_serial(port: str, baud: int, timeout: float) -> serial.Serial:
|
||||
try:
|
||||
return serial.Serial(
|
||||
port=port,
|
||||
baudrate=baud,
|
||||
bytesize=serial.EIGHTBITS,
|
||||
parity=serial.PARITY_NONE,
|
||||
stopbits=serial.STOPBITS_ONE,
|
||||
timeout=timeout,
|
||||
)
|
||||
except serial.SerialException as exc:
|
||||
print(f"Serial error: {exc}", file=sys.stderr)
|
||||
print("Check:", file=sys.stderr)
|
||||
print(f" 1. Device node {port} exists", file=sys.stderr)
|
||||
print(" 2. User has access (sudo or dialout group)", file=sys.stderr)
|
||||
print(" 3. Wiring / USB-TTL connection", file=sys.stderr)
|
||||
raise SystemExit(1) from exc
|
||||
|
||||
|
||||
def main(argv: list[str] | None = None) -> int:
|
||||
parser = argparse.ArgumentParser(
|
||||
description="UART finger controller (search/connect/run/stop force stream)"
|
||||
)
|
||||
parser.add_argument("--port", default="/dev/ttyHS1", help="Serial device (default: /dev/ttyHS1)")
|
||||
parser.add_argument("--baud", type=int, default=115200, help="Baud rate (default: 115200)")
|
||||
parser.add_argument(
|
||||
"--timeout",
|
||||
type=float,
|
||||
default=0.1,
|
||||
help="Serial readline timeout seconds (default: 0.1)",
|
||||
)
|
||||
parser.add_argument("--config", "-c", default="finger", help="Orisys config name or path")
|
||||
parser.add_argument(
|
||||
"--camera-id",
|
||||
type=int,
|
||||
default=0,
|
||||
help="Default camera index when connect has no argument (ignored if --video is set)",
|
||||
)
|
||||
parser.add_argument(
|
||||
"--video",
|
||||
default=None,
|
||||
help="Default video file for connect (offline debug without a camera)",
|
||||
)
|
||||
parser.add_argument(
|
||||
"--max-camera-index",
|
||||
type=int,
|
||||
default=10,
|
||||
help="Highest index probed by search",
|
||||
)
|
||||
parser.add_argument(
|
||||
"--loop-video",
|
||||
dest="loop_video",
|
||||
action="store_true",
|
||||
default=True,
|
||||
help="Loop video file on EOF during run (default: on)",
|
||||
)
|
||||
parser.add_argument(
|
||||
"--no-loop-video",
|
||||
dest="loop_video",
|
||||
action="store_false",
|
||||
help="Stop run with OK eof when video file ends",
|
||||
)
|
||||
parser.add_argument(
|
||||
"--show",
|
||||
action="store_true",
|
||||
help="Show UV unrolled OpenCV window (local debug only)",
|
||||
)
|
||||
parser.add_argument("--verbose", action="store_true", help="Echo UART traffic and SDK logs")
|
||||
args = parser.parse_args(argv)
|
||||
|
||||
if args.video:
|
||||
video_path = Path(args.video)
|
||||
if not video_path.is_file():
|
||||
print(f"Video file not found: {args.video}", file=sys.stderr)
|
||||
return 1
|
||||
default_source: Source = str(video_path.resolve())
|
||||
if self.autostart:
|
||||
self.cmd_connect(["all"])
|
||||
if self.bank.runtimes:
|
||||
self.cmd_run([])
|
||||
else:
|
||||
default_source = args.camera_id
|
||||
raise RuntimeError("autostart could not connect any sensor")
|
||||
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
|
||||
|
||||
ser = open_serial(args.port, args.baud, args.timeout)
|
||||
controller = UartFingerController(
|
||||
ser,
|
||||
config=args.config,
|
||||
default_source=default_source,
|
||||
max_camera_index=args.max_camera_index,
|
||||
loop_video=args.loop_video,
|
||||
show=args.show,
|
||||
verbose=args.verbose,
|
||||
|
||||
def open_serial(port, baud, timeout, write_timeout=1.0):
|
||||
try:
|
||||
import serial
|
||||
except ImportError as exc:
|
||||
raise RuntimeError("pyserial is required: python -m pip install pyserial") from exc
|
||||
return serial.Serial(
|
||||
port=port, baudrate=baud, bytesize=serial.EIGHTBITS, parity=serial.PARITY_NONE,
|
||||
stopbits=serial.STOPBITS_ONE, timeout=timeout, write_timeout=write_timeout,
|
||||
exclusive=True,
|
||||
)
|
||||
|
||||
print(f"UART finger controller on {args.port} @ {args.baud} bps", flush=True)
|
||||
print(f"Default source: {default_source}", flush=True)
|
||||
print("Commands: search | connect [source|file] | run | stop | quit", flush=True)
|
||||
|
||||
def main(argv=None):
|
||||
parser = argparse.ArgumentParser(description="UART finger control and read-only streaming")
|
||||
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")
|
||||
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("--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")
|
||||
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 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,
|
||||
]
|
||||
controller = UartFingerController(
|
||||
None, sources=sources, configs=configs, max_camera_index=args.max_camera_index,
|
||||
loop_video=args.loop_video, show=args.show, verbose=args.verbose,
|
||||
autostart=args.autostart, read_only=args.read_only,
|
||||
)
|
||||
except ValueError as exc:
|
||||
parser.error(str(exc))
|
||||
ser = None
|
||||
result = 0
|
||||
try:
|
||||
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)
|
||||
controller.serve()
|
||||
except KeyboardInterrupt:
|
||||
print("\nInterrupted.", flush=True)
|
||||
try:
|
||||
controller.reply("OK quit")
|
||||
except Exception: # noqa: BLE001
|
||||
pass
|
||||
print("Interrupted.", file=sys.stderr)
|
||||
except Exception:
|
||||
logger.exception("UART service failed on %s", args.port)
|
||||
result = 1
|
||||
finally:
|
||||
try:
|
||||
controller.close_runtime()
|
||||
if ser.is_open:
|
||||
except Exception:
|
||||
logger.exception("Runtime cleanup failed")
|
||||
result = 1
|
||||
finally:
|
||||
if ser is not None:
|
||||
ser.close()
|
||||
print("Serial closed.", flush=True)
|
||||
return 0
|
||||
return result
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
|
||||
@@ -0,0 +1,136 @@
|
||||
"""Single-threaded finger acquisition API, independent of UART and the CLI.
|
||||
|
||||
Sensor IDs are 1-based, fixed by source order. Calls and runtime ownership belong
|
||||
to one thread; the caller must 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
|
||||
|
||||
import logging
|
||||
import math
|
||||
from dataclasses import dataclass
|
||||
from pathlib import Path
|
||||
|
||||
Source = int | str
|
||||
logger = logging.getLogger(__name__)
|
||||
|
||||
|
||||
def parse_source(token: str) -> Source:
|
||||
text = token.strip()
|
||||
if not text:
|
||||
raise ValueError("empty source")
|
||||
return int(text) if text.isdigit() or (text.startswith("-") and text[1:].isdigit()) else text
|
||||
|
||||
|
||||
def is_video_file(source: Source) -> bool:
|
||||
from orisys.capture import is_live_camera
|
||||
|
||||
return not is_live_camera(source) and not isinstance(source, int)
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class ForceReading:
|
||||
sensor_id: int
|
||||
normal: float
|
||||
shear_x: float
|
||||
shear_y: float
|
||||
units: tuple[str, str, str]
|
||||
image: object
|
||||
|
||||
|
||||
class FingerSensorBank:
|
||||
"""Own 1–4 independent runtimes. read() returns None at EOF/capture failure.
|
||||
|
||||
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):
|
||||
sources = tuple(parse_source(str(source)) for source in sources)
|
||||
configs = tuple(configs)
|
||||
if not 1 <= len(sources) <= 4:
|
||||
raise ValueError("provide 1 to 4 sources")
|
||||
if len(configs) not in (1, len(sources)):
|
||||
raise ValueError("provide one config or one config per source")
|
||||
if any(isinstance(source, int) and source < 0 for source in sources):
|
||||
raise ValueError("camera index must be non-negative")
|
||||
if len(configs) == 1:
|
||||
configs = configs * len(sources)
|
||||
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:
|
||||
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,
|
||||
)
|
||||
try:
|
||||
ok, _ = runtime.warmup(color=False)
|
||||
if not ok:
|
||||
raise RuntimeError("warmup could not read a frame")
|
||||
except BaseException:
|
||||
runtime.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")
|
||||
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]):
|
||||
import cv2
|
||||
|
||||
# One rewind attempt per read prevents an empty file from spinning forever.
|
||||
if runtime.capture is not None and runtime.capture.set(cv2.CAP_PROP_POS_FRAMES, 0):
|
||||
runtime.tactile.reset()
|
||||
ok, image = runtime.grab_and_process(color=False)
|
||||
if not ok:
|
||||
return None
|
||||
tactile = runtime.tactile
|
||||
tactile.compute_deformation()
|
||||
tactile.compute_contact()
|
||||
forces = tuple(float(value) for value in tactile.read_info(
|
||||
tactile.info.FNORMAL, tactile.info.FSHEARX, tactile.info.FSHEARY,
|
||||
))
|
||||
if not all(math.isfinite(value) for value in forces):
|
||||
raise RuntimeError("non-finite force measurement")
|
||||
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)
|
||||
except Exception as exc:
|
||||
logger.exception("Sensor %s cleanup failed", sensor_id)
|
||||
first_error = first_error or exc
|
||||
if first_error is not None:
|
||||
raise first_error
|
||||
@@ -0,0 +1,385 @@
|
||||
"""Hardware-free protocol and sensor lifecycle regression tests."""
|
||||
|
||||
import io
|
||||
import sys
|
||||
import types
|
||||
import unittest
|
||||
from contextlib import redirect_stderr
|
||||
from pathlib import Path
|
||||
from unittest.mock import patch
|
||||
|
||||
SERIAL_DIR = Path(__file__).resolve().parents[1] / "serial"
|
||||
sys.path.insert(0, str(SERIAL_DIR))
|
||||
import example_uart as uart
|
||||
from finger_api import FingerSensorBank
|
||||
|
||||
|
||||
class ByteSerial:
|
||||
def __init__(self, data=b""):
|
||||
self.input = bytearray(data)
|
||||
self.output = []
|
||||
|
||||
@property
|
||||
def in_waiting(self):
|
||||
return len(self.input)
|
||||
|
||||
def read(self, size=1):
|
||||
result = bytes(self.input[:size])
|
||||
del self.input[:size]
|
||||
return result
|
||||
|
||||
def readline(self):
|
||||
end = self.input.find(b"\n")
|
||||
return self.read(end + 1 if end >= 0 else len(self.input))
|
||||
|
||||
def write(self, data):
|
||||
self.output.append(data.decode().rstrip("\n"))
|
||||
return len(data)
|
||||
|
||||
def flush(self):
|
||||
pass
|
||||
|
||||
|
||||
class FakeTactile:
|
||||
info = types.SimpleNamespace(FNORMAL=1, FSHEARX=2, FSHEARY=3)
|
||||
force_units = ("raw", "raw", "raw")
|
||||
|
||||
def compute_deformation(self):
|
||||
pass
|
||||
|
||||
def compute_contact(self):
|
||||
pass
|
||||
|
||||
def read_info(self, *args):
|
||||
return 1.25, -2.5, 3.75
|
||||
|
||||
def reset(self):
|
||||
pass
|
||||
|
||||
|
||||
class FakeRuntime:
|
||||
def __init__(self, *, warmup_error=False, warmup_ok=True, grab_error=False):
|
||||
self.tactile = FakeTactile()
|
||||
self.closed = False
|
||||
self.warmup_error = warmup_error
|
||||
self.warmup_ok = warmup_ok
|
||||
self.grab_error = grab_error
|
||||
self.capture = types.SimpleNamespace(set=lambda *args: True)
|
||||
self.frames = 1
|
||||
|
||||
def warmup(self, **kwargs):
|
||||
if self.warmup_error:
|
||||
raise RuntimeError("warmup failed")
|
||||
return self.warmup_ok, None
|
||||
|
||||
def grab_and_process(self, **kwargs):
|
||||
if self.grab_error:
|
||||
raise RuntimeError("processing failed")
|
||||
if self.frames:
|
||||
self.frames -= 1
|
||||
return True, None
|
||||
return False, None
|
||||
|
||||
def close(self):
|
||||
self.closed = True
|
||||
|
||||
|
||||
class HardwareFreeTest(unittest.TestCase):
|
||||
def setUp(self):
|
||||
stderr = redirect_stderr(io.StringIO())
|
||||
stderr.__enter__()
|
||||
self.addCleanup(stderr.__exit__, None, None, None)
|
||||
finger = types.ModuleType("orisys.finger")
|
||||
finger.FingerRuntime = types.SimpleNamespace(open=lambda **kwargs: FakeRuntime())
|
||||
capture = types.ModuleType("orisys.capture")
|
||||
capture.is_live_camera = lambda source: isinstance(source, int) or str(source).startswith("/dev/video")
|
||||
capture.list_cameras = lambda **kwargs: []
|
||||
package = types.ModuleType("orisys")
|
||||
package.finger, package.capture = finger, capture
|
||||
modules = patch.dict(sys.modules, {
|
||||
"orisys": package, "orisys.finger": finger, "orisys.capture": capture,
|
||||
})
|
||||
modules.start()
|
||||
self.addCleanup(modules.stop)
|
||||
|
||||
|
||||
class ExistingRegressionTests(HardwareFreeTest):
|
||||
def test_partial_command_waits_for_terminator(self):
|
||||
ser = ByteSerial(b"st")
|
||||
controller = uart.UartFingerController(ser)
|
||||
self.assertIsNone(controller._poll_command_line(blocking=False))
|
||||
ser.input.extend(b"op\n")
|
||||
self.assertEqual(controller._poll_command_line(blocking=False), "stop")
|
||||
|
||||
def test_cr_only_terminal_commands(self):
|
||||
controller = uart.UartFingerController(ByteSerial(b"stop\rrun\r"))
|
||||
self.assertEqual(controller._poll_command_line(blocking=False), "stop")
|
||||
self.assertEqual(controller._poll_command_line(blocking=False), "run")
|
||||
|
||||
def test_failed_warmup_releases_runtime(self):
|
||||
runtime = FakeRuntime(warmup_error=True)
|
||||
ser = ByteSerial()
|
||||
controller = uart.UartFingerController(ser)
|
||||
with patch("orisys.finger.FingerRuntime.open", return_value=runtime):
|
||||
controller.cmd_connect([])
|
||||
self.assertTrue(runtime.closed)
|
||||
self.assertTrue(ser.output[-1].startswith("ERR connect"))
|
||||
|
||||
def test_unsuccessful_warmup_is_not_connected(self):
|
||||
runtime = FakeRuntime(warmup_ok=False)
|
||||
ser = ByteSerial()
|
||||
controller = uart.UartFingerController(ser)
|
||||
with patch("orisys.finger.FingerRuntime.open", return_value=runtime):
|
||||
controller.cmd_connect([])
|
||||
self.assertTrue(ser.output[-1].startswith("ERR connect"))
|
||||
self.assertTrue(runtime.closed)
|
||||
|
||||
|
||||
class MultiSensorTests(HardwareFreeTest):
|
||||
def test_autostart_service_streams_one_through_four_sensors_then_stops(self):
|
||||
for count in range(1, 5):
|
||||
with self.subTest(count=count):
|
||||
ser = ByteSerial()
|
||||
controller = uart.UartFingerController(ser, sources=list(range(count)), autostart=True)
|
||||
original_write = ser.write
|
||||
def write(data, original_write=original_write, ser=ser, count=count):
|
||||
size = original_write(data)
|
||||
if data.startswith(b"force,") and sum(line.startswith("force,") for line in ser.output) == count:
|
||||
ser.input.extend(b"stop all\nquit\n")
|
||||
return size
|
||||
ser.write = write
|
||||
controller.serve()
|
||||
self.assertEqual(sum(line.startswith("force,") for line in ser.output), count)
|
||||
self.assertEqual(ser.output[-1], "OK quit")
|
||||
self.assertFalse(controller._streaming)
|
||||
controller.close_runtime()
|
||||
|
||||
def test_one_through_four_sensors_have_identifiable_readings(self):
|
||||
for count in range(1, 5):
|
||||
with self.subTest(count=count):
|
||||
ser = ByteSerial()
|
||||
controller = uart.UartFingerController(ser, sources=list(range(count)))
|
||||
controller.cmd_connect(["all"])
|
||||
controller._dispatch("read all")
|
||||
forces = [line for line in ser.output if line.startswith("force,")]
|
||||
expected = ["force,1.250000,-2.500000,3.750000"] if count == 1 else [
|
||||
f"force,{i},1.250000,-2.500000,3.750000" for i in range(1, count + 1)
|
||||
]
|
||||
self.assertEqual(forces, expected)
|
||||
self.assertFalse(controller._streaming)
|
||||
controller.close_runtime()
|
||||
|
||||
def test_selective_stop_and_reconnect_leave_other_sensor_running(self):
|
||||
controller = uart.UartFingerController(ByteSerial(), sources=[0, 2, 4, 6])
|
||||
controller.cmd_connect(["all"])
|
||||
controller._dispatch("run all")
|
||||
controller._dispatch("stop 2")
|
||||
self.assertEqual(controller._streaming, {1, 3, 4})
|
||||
old = controller.bank.runtimes[3]
|
||||
controller._dispatch("connect 3 8")
|
||||
self.assertTrue(old.closed)
|
||||
self.assertEqual(controller._streaming, {1, 4})
|
||||
self.assertEqual(controller.bank.sources[3], 8)
|
||||
|
||||
def test_failed_sensor_does_not_stop_healthy_sensor(self):
|
||||
ser = ByteSerial()
|
||||
controller = uart.UartFingerController(ser, sources=[0, 2])
|
||||
controller.cmd_connect(["all"])
|
||||
controller.bank.runtimes[1].grab_error = True
|
||||
controller.cmd_run([])
|
||||
for sensor_id in sorted(controller._streaming):
|
||||
controller._read_one(sensor_id, context="run")
|
||||
self.assertIn("ERR run 1 processing failed", ser.output)
|
||||
self.assertIn("force,2,1.250000,-2.500000,3.750000", ser.output)
|
||||
self.assertEqual(controller._streaming, {2})
|
||||
|
||||
def test_connect_all_retains_successes_when_one_source_fails(self):
|
||||
ser = ByteSerial()
|
||||
controller = uart.UartFingerController(ser, sources=[0, 2, 4])
|
||||
def open_runtime(**kwargs):
|
||||
if kwargs["video"] == 2:
|
||||
raise RuntimeError("camera unavailable")
|
||||
return FakeRuntime()
|
||||
with patch("orisys.finger.FingerRuntime.open", side_effect=open_runtime):
|
||||
controller.cmd_connect(["all"])
|
||||
self.assertEqual(set(controller.bank.runtimes), {1, 3})
|
||||
self.assertIn("ERR connect 2 camera unavailable", ser.output)
|
||||
|
||||
def test_rejects_duplicate_live_source_without_closing_other_camera(self):
|
||||
controller = uart.UartFingerController(ByteSerial(), sources=[0, 2])
|
||||
controller.cmd_connect(["all"])
|
||||
original = controller.bank.runtimes[2]
|
||||
controller._dispatch("connect 2 0")
|
||||
self.assertIs(controller.bank.runtimes[2], original)
|
||||
self.assertFalse(original.closed)
|
||||
|
||||
def test_config_is_selected_per_sensor(self):
|
||||
bank = FingerSensorBank([0, 2], configs=["left.json", "right.json"])
|
||||
seen = []
|
||||
def open_runtime(**kwargs):
|
||||
seen.append(kwargs["config"])
|
||||
return FakeRuntime()
|
||||
with patch("orisys.finger.FingerRuntime.open", side_effect=open_runtime):
|
||||
bank.connect(1)
|
||||
bank.connect(2)
|
||||
self.assertEqual(seen, ["left.json", "right.json"])
|
||||
self.assertEqual(bank.read(1).units, ("raw", "raw", "raw"))
|
||||
|
||||
def test_invalid_sensor_count_and_config_count(self):
|
||||
for sources, configs in [([], ["finger"]), (range(5), ["finger"]), ([0, 2], ["a", "b", "c"])]:
|
||||
with self.assertRaises(ValueError):
|
||||
FingerSensorBank(sources, configs=configs)
|
||||
|
||||
def test_invalid_command_id_and_unconnected_run_are_errors(self):
|
||||
ser = ByteSerial()
|
||||
controller = uart.UartFingerController(ser, sources=[0, 2])
|
||||
for command in ["run 0", "read 3", "stop -1", "connect 3 4", "run all", "run 2 extra"]:
|
||||
self.assertTrue(controller._dispatch(command))
|
||||
self.assertTrue(ser.output[-1].startswith("ERR "))
|
||||
|
||||
|
||||
class ProtocolTests(HardwareFreeTest):
|
||||
def test_repeated_run_is_rejected_without_recursion(self):
|
||||
ser = ByteSerial()
|
||||
controller = uart.UartFingerController(ser)
|
||||
controller.cmd_connect([])
|
||||
controller._dispatch("run")
|
||||
controller._dispatch("run")
|
||||
self.assertEqual(ser.output[-1], "ERR run already running")
|
||||
self.assertEqual(controller._streaming, {1})
|
||||
|
||||
def test_camera_search_failure_is_reported_without_exiting(self):
|
||||
ser = ByteSerial()
|
||||
controller = uart.UartFingerController(ser)
|
||||
with patch("orisys.capture.list_cameras", side_effect=OSError("camera scan failed")):
|
||||
self.assertTrue(controller._dispatch("search"))
|
||||
self.assertEqual(ser.output[-1], "ERR search camera scan failed")
|
||||
|
||||
def test_legacy_connect_and_batched_stop_quit(self):
|
||||
ser = ByteSerial(b"connect 2\r\nrun\nstop\nquit\n")
|
||||
controller = uart.UartFingerController(ser)
|
||||
controller.serve()
|
||||
self.assertEqual(ser.output, ["OK ready", "OK connect 2 (camera)", "OK run", "OK stop", "OK quit"])
|
||||
self.assertFalse(controller._streaming)
|
||||
|
||||
def test_stop_is_processed_before_next_frame(self):
|
||||
ser = ByteSerial(b"stop\nquit\n")
|
||||
controller = uart.UartFingerController(ser)
|
||||
controller.cmd_connect([])
|
||||
controller.cmd_run([])
|
||||
controller.serve()
|
||||
self.assertFalse(any(line.startswith("force,") for line in ser.output))
|
||||
|
||||
def test_read_only_autostarts_and_ignores_host_commands(self):
|
||||
ser = ByteSerial(b"quit\nstop\nconnect 9\n" + b"a" * 1100 + b"\n")
|
||||
controller = uart.UartFingerController(ser, read_only=True)
|
||||
original_write = ser.write
|
||||
def write(data):
|
||||
size = original_write(data)
|
||||
if data.startswith(b"force,"):
|
||||
raise KeyboardInterrupt()
|
||||
return size
|
||||
ser.write = write
|
||||
with self.assertRaises(KeyboardInterrupt):
|
||||
controller.serve()
|
||||
self.assertIn("OK run", ser.output)
|
||||
self.assertIn("force,1.250000,-2.500000,3.750000", ser.output)
|
||||
self.assertNotIn("OK quit", ser.output)
|
||||
self.assertFalse(any(line.startswith("ERR ") for line in ser.output))
|
||||
|
||||
def test_long_line_is_discarded_until_delimiter(self):
|
||||
ser = ByteSerial(b"a" * 1025)
|
||||
controller = uart.UartFingerController(ser)
|
||||
self.assertIsNone(controller._poll_command_line(blocking=False))
|
||||
ser.input.extend(b"quit\nstop\r")
|
||||
self.assertEqual(controller._poll_command_line(blocking=False), "stop")
|
||||
self.assertEqual(ser.output, ["ERR command line too long (max 1024 bytes)"])
|
||||
self.assertFalse(controller._should_quit)
|
||||
|
||||
def test_quoted_paths_and_malformed_quotes(self):
|
||||
controller = uart.UartFingerController(ByteSerial(), sources=[0, 2])
|
||||
with patch.object(controller.bank, "connect", return_value="/tmp/a b.mp4") as connect:
|
||||
controller._dispatch('connect 2 "/tmp/a b.mp4"')
|
||||
connect.assert_called_once_with(2, "/tmp/a b.mp4")
|
||||
controller._dispatch('connect "broken')
|
||||
self.assertTrue(controller.ser.output[-1].startswith("ERR command"))
|
||||
|
||||
def test_non_finite_measurements_are_not_published(self):
|
||||
ser = ByteSerial()
|
||||
controller = uart.UartFingerController(ser)
|
||||
controller.cmd_connect([])
|
||||
controller.bank.runtimes[1].tactile.read_info = lambda *args: (float("nan"), 0, 0)
|
||||
controller._dispatch("read")
|
||||
self.assertEqual(ser.output[-1], "ERR read non-finite force measurement")
|
||||
self.assertFalse(any(line.startswith("force,") for line in ser.output))
|
||||
|
||||
def test_serial_short_write_is_a_transport_failure(self):
|
||||
ser = ByteSerial()
|
||||
ser.write = lambda data: len(data) - 1
|
||||
with self.assertRaises(OSError):
|
||||
uart.UartFingerController(ser).reply("OK ready")
|
||||
|
||||
def test_error_newlines_cannot_inject_protocol_records(self):
|
||||
ser = ByteSerial()
|
||||
uart.UartFingerController(ser).reply("ERR read bad\nOK quit\r\n")
|
||||
self.assertEqual(ser.output, ["ERR read bad OK quit"])
|
||||
|
||||
def test_empty_looping_video_retries_once_then_reports_eof(self):
|
||||
ser = ByteSerial()
|
||||
controller = uart.UartFingerController(ser)
|
||||
runtime = FakeRuntime()
|
||||
runtime.frames = 0
|
||||
controller.bank.sources[1] = "empty.mp4"
|
||||
controller.bank.runtimes[1] = runtime
|
||||
controller._dispatch("read")
|
||||
self.assertEqual(ser.output, ["OK eof"])
|
||||
|
||||
def test_close_attempts_every_sensor_even_when_one_close_fails(self):
|
||||
bank = FingerSensorBank([0, 2])
|
||||
bank.connect(1)
|
||||
bank.connect(2)
|
||||
second = bank.runtimes[2]
|
||||
bank.runtimes[1].close = lambda: (_ for _ in ()).throw(RuntimeError("close failed"))
|
||||
with self.assertRaisesRegex(RuntimeError, "close failed"):
|
||||
bank.close()
|
||||
self.assertTrue(second.closed)
|
||||
self.assertEqual(bank.runtimes, {})
|
||||
|
||||
|
||||
class CliTests(HardwareFreeTest):
|
||||
def test_open_serial_has_finite_write_timeout_and_no_flow_control(self):
|
||||
serial = types.ModuleType("serial")
|
||||
serial.EIGHTBITS, serial.PARITY_NONE, serial.STOPBITS_ONE = 8, "N", 1
|
||||
serial.Serial = types.SimpleNamespace()
|
||||
with patch.dict(sys.modules, {"serial": serial}), patch.object(serial, "Serial") as factory:
|
||||
uart.open_serial("/dev/ttyHS1", 115200, 0.1, 0.5)
|
||||
factory.assert_called_once_with(
|
||||
port="/dev/ttyHS1", baudrate=115200, bytesize=8, parity="N",
|
||||
stopbits=1, timeout=0.1, write_timeout=0.5, exclusive=True,
|
||||
)
|
||||
|
||||
def test_invalid_options_do_not_open_uart(self):
|
||||
for args in [["--timeout", "0"], ["--write-timeout", "nan"], ["--sources", "0", "1", "2", "3", "4"],
|
||||
["--sources", "0", "2", "--config", "a", "--config", "b", "--config", "c"]]:
|
||||
with self.subTest(args=args), patch.object(uart, "open_serial") as open_serial:
|
||||
with self.assertRaises(SystemExit) as exit_status:
|
||||
uart.main(args)
|
||||
self.assertEqual(exit_status.exception.code, 2)
|
||||
open_serial.assert_not_called()
|
||||
|
||||
def test_transport_failure_returns_nonzero_and_closes_all_resources(self):
|
||||
ser = ByteSerial()
|
||||
ser.closed = False
|
||||
ser.close = lambda: setattr(ser, "closed", True)
|
||||
def serve(controller):
|
||||
controller.cmd_connect(["all"])
|
||||
self.runtimes = list(controller.bank.runtimes.values())
|
||||
raise OSError("UART disconnected")
|
||||
with patch.object(uart, "open_serial", return_value=ser), patch.object(uart.UartFingerController, "serve", serve):
|
||||
self.assertEqual(uart.main(["--sources", "0", "2"]), 1)
|
||||
self.assertTrue(ser.closed)
|
||||
self.assertTrue(all(runtime.closed for runtime in self.runtimes))
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
unittest.main()
|
||||
Reference in New Issue
Block a user