uart example for giner

serial/example_uart.py

@@ -0,0 +1,461 @@
+#!/usr/bin/env python3
+"""UART finger controller for Radxa / Dragon Q6A (sensor-side receiver).
+
+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
+"""
+
+from __future__ import annotations
+
+import argparse
+import os
+import shlex
+import sys
+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
+
+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",
+    }
+
+
+class UartFingerController:
+    """Serial command dispatcher around a single FingerRuntime."""
+
+    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 = ser
+        self.config = config
+        self.default_source = default_source
+        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._should_quit = False
+
+    def reply(self, line: str) -> None:
+        text = line.rstrip("\r\n") + "\n"
+        self.ser.write(text.encode("utf-8"))
+        self.ser.flush()
+        if self.verbose:
+            print(f"<< {text.rstrip()}", flush=True)
+
+    def close_runtime(self) -> None:
+        self._streaming = False
+        if self.runtime is not None:
+            try:
+                self.runtime.close()
+            except Exception:  # noqa: BLE001
+                pass
+            self.runtime = None
+            self._connected_source = None
+        if self.show:
+            try:
+                cv2.destroyAllWindows()
+            except Exception:  # noqa: BLE001
+                pass
+
+    def _resolve_connect_source(self, args: list[str]) -> Source:
+        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:
+        try:
+            cameras = list_cameras(max_index=self.max_camera_index)
+        except Exception as exc:  # noqa: BLE001
+            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}")
+            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}")
+            return
+
+        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}")
+                return
+
+        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
+
+        self.runtime = runtime
+        self._connected_source = source
+        kind = "file" if _is_video_file(source) else "camera"
+        self.reply(f"OK connect {source} ({kind})")
+
+    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
+        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."""
+        try:
+            parts = shlex.split(line)
+        except ValueError:
+            parts = line.split()
+        if not parts:
+            return True
+        cmd = parts[0].lower()
+        args = parts[1:]
+
+        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 == "stop":
+            self.cmd_stop(args)
+        elif cmd in ("quit", "exit"):
+            self.cmd_quit(args)
+            return False
+        else:
+            self.reply(f"ERR unknown {cmd}")
+        return not self._should_quit
+
+    def serve(self) -> None:
+        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())
+    else:
+        default_source = args.camera_id
+
+    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,
+    )
+
+    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)
+
+    try:
+        controller.serve()
+    except KeyboardInterrupt:
+        print("\nInterrupted.", flush=True)
+        try:
+            controller.reply("OK quit")
+        except Exception:  # noqa: BLE001
+            pass
+    finally:
+        controller.close_runtime()
+        if ser.is_open:
+            ser.close()
+            print("Serial closed.", flush=True)
+    return 0
+
+
+if __name__ == "__main__":
+    raise SystemExit(main())