ykcheungab
273 lines
10 KiB
Python
273 lines
10 KiB
Python
#!/usr/bin/env python3
|
|
"""Headless UART finger service for 1-4 sensors; see ../docs/uart.md.
|
|
|
|
The login console and data UART must be separate ports. 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.
|
|
"""
|
|
|
|
from __future__ import annotations
|
|
|
|
import argparse
|
|
import logging
|
|
import math
|
|
import os
|
|
import shlex
|
|
import sys
|
|
from collections import deque
|
|
|
|
os.environ.setdefault("OPENCV_VIDEOIO_MSMF_ENABLE_HW_TRANSFORMS", "0")
|
|
|
|
from finger_api import FingerSensorBank, parse_source
|
|
|
|
logger = logging.getLogger(__name__)
|
|
|
|
|
|
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,
|
|
):
|
|
self.ser = ser
|
|
self.bank = FingerSensorBank(config=config, verbose=verbose)
|
|
self.loop_video = loop_video
|
|
self.show = show
|
|
self.verbose = verbose
|
|
self._streaming = False
|
|
self._should_quit = False
|
|
self._partial = bytearray()
|
|
self._lines = deque()
|
|
self._discard_line = False
|
|
|
|
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()}", file=sys.stderr, flush=True)
|
|
|
|
def close_runtime(self):
|
|
self._streaming = False
|
|
try:
|
|
self.bank.close()
|
|
finally:
|
|
if self.show:
|
|
import cv2
|
|
|
|
cv2.destroyAllWindows()
|
|
|
|
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)
|
|
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 _read_round(self):
|
|
try:
|
|
readings = self.bank.read_all(loop_video=self.loop_video)
|
|
except EOFError as exc:
|
|
self._streaming = False
|
|
self.reply(f"OK eof {exc}")
|
|
return
|
|
except Exception as exc:
|
|
logger.exception("Acquisition round failed")
|
|
self._streaming = False
|
|
self.reply(f"ERR read {exc}")
|
|
return
|
|
for reading in readings:
|
|
self.reply(
|
|
f"force,{reading.sensor_id},{reading.normal:.6f},"
|
|
f"{reading.shear_x:.6f},{reading.shear_y:.6f}"
|
|
)
|
|
if self.show:
|
|
self._show(reading)
|
|
if self._should_quit:
|
|
return
|
|
|
|
def _show(self, reading):
|
|
import cv2
|
|
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._streaming = False
|
|
self._should_quit = True
|
|
self.reply("OK quit")
|
|
elif key == ord("r"):
|
|
tactile.reset()
|
|
except Exception:
|
|
logger.exception("Sensor %s display failed", reading.sensor_id)
|
|
|
|
def _dispatch(self, line):
|
|
try:
|
|
parts = shlex.split(line)
|
|
except ValueError as exc:
|
|
self.reply(f"ERR command {exc}")
|
|
return True
|
|
if not parts:
|
|
return True
|
|
command = parts[0].lower()
|
|
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}")
|
|
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")
|
|
elif command == "start":
|
|
if not self.bank.runtimes:
|
|
self.reply("ERR start no sensors added")
|
|
else:
|
|
self._streaming = True
|
|
self.reply("OK start")
|
|
elif command == "stop":
|
|
self._streaming = False
|
|
self.reply("OK stop")
|
|
elif command == "read":
|
|
self._read_round()
|
|
elif command == "quit":
|
|
self._streaming = False
|
|
self._should_quit = True
|
|
self.reply("OK quit")
|
|
return not self._should_quit
|
|
|
|
def serve(self, *, start=False):
|
|
if start and not self.bank.runtimes:
|
|
raise RuntimeError("no sensors added")
|
|
self.reply(f"OK ready {len(self.bank.sources)}")
|
|
self._streaming = start
|
|
while not self._should_quit:
|
|
for _ in range(16):
|
|
line = self._poll_command_line(blocking=not self._streaming)
|
|
if line is None:
|
|
break
|
|
if not self._dispatch(line):
|
|
return
|
|
if self._streaming:
|
|
self._read_round()
|
|
|
|
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,
|
|
)
|
|
|
|
|
|
def main(argv=None):
|
|
parser = argparse.ArgumentParser(description="Manual UART finger service: add, start, stop, read, 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")
|
|
parser.add_argument("--write-timeout", type=float, default=1.0)
|
|
parser.add_argument("--config", "-c", default="finger", help="default config for add commands")
|
|
parser.add_argument("--setup", help="JSON file listing sources, configs and calibrations to connect")
|
|
parser.add_argument("--start", action="store_true", help="start output after loading --setup")
|
|
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")
|
|
args = parser.parse_args(argv)
|
|
if args.start and not args.setup:
|
|
parser.error("--start requires --setup")
|
|
if args.baud <= 0:
|
|
parser.error("baud must be positive")
|
|
if any(not math.isfinite(t) or t <= 0 for t in (args.timeout, args.write_timeout)):
|
|
parser.error("timeouts must be finite and positive")
|
|
controller = UartFingerController(
|
|
None, config=args.config, loop_video=args.loop_video,
|
|
show=args.show, verbose=args.verbose,
|
|
)
|
|
ser = None
|
|
result = 0
|
|
try:
|
|
if args.setup:
|
|
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)
|
|
controller.serve(start=args.start)
|
|
except KeyboardInterrupt:
|
|
print("Interrupted.", file=sys.stderr)
|
|
except Exception:
|
|
logger.exception("UART service failed on %s", args.port)
|
|
result = 1
|
|
finally:
|
|
try:
|
|
controller.close_runtime()
|
|
except Exception:
|
|
logger.exception("Runtime cleanup failed")
|
|
result = 1
|
|
finally:
|
|
if ser is not None:
|
|
ser.close()
|
|
return result
|
|
|
|
|
|
if __name__ == "__main__":
|
|
raise SystemExit(main())
|