391 lines
16 KiB
Python
391 lines
16 KiB
Python
"""MovementDispatcher — Gemini voice → locomotion (N2 Phase 3).
|
||
|
||
The Marcus phrase-confirmation pattern, ported to Sanad. Gemini Live runs in a
|
||
child subprocess; the parent supervisor (gemini/subprocess.py) parses Gemini's
|
||
OWN spoken output into BOT: transcript lines and fires `on_bot_text(line)` here.
|
||
|
||
Flow:
|
||
Gemini speaks a canonical confirmation phrase ("Turning right." / "أستدير
|
||
يميناً.") → on_bot_text() matches it against data/motions/instruction.json
|
||
→ enqueues a canonical command → a worker thread drives loco_controller
|
||
(discrete steps that self-terminate with StopMove).
|
||
|
||
Gating: every dispatch is gated on `recognition_state.movement_enabled` (the
|
||
"Enable Gemini movement" dashboard toggle) — SEPARATE from the manual
|
||
"Enable movement" arm flag (loco_controller._armed). When the flag is off,
|
||
matches are dropped. "stop" is honoured immediately (cross-thread abort + drain
|
||
+ E-STOP) regardless of cooldown.
|
||
|
||
Safety: discrete `loco.step()` self-stops; velocity caps live in LocoController;
|
||
N-step / N-degree commands are bounded by instruction.json (max_steps,
|
||
max_degrees) and check the abort flag + enable gate between each step. Numbers
|
||
are kept verbatim but the degrees→steps and steps mapping is APPROXIMATE and
|
||
must be calibrated on the real robot.
|
||
"""
|
||
|
||
from __future__ import annotations
|
||
|
||
import json
|
||
import queue
|
||
import re
|
||
import threading
|
||
import time
|
||
from pathlib import Path
|
||
from typing import Any, List, Optional
|
||
|
||
from Project.Sanad.core.logger import get_logger
|
||
|
||
log = get_logger("movement_dispatch")
|
||
|
||
_SENTINEL = object()
|
||
_STATE_CACHE_TTL = 0.5 # seconds — re-read recognition_state at most this often
|
||
|
||
# Map a resolved canonical command to a LocoController discrete-step direction.
|
||
_FIXED_STEP = {
|
||
"move forward": "forward",
|
||
"move backward": "backward",
|
||
"turn right": "rotate_right",
|
||
"turn left": "rotate_left",
|
||
"slide left": "slide_left",
|
||
"slide right": "slide_right",
|
||
}
|
||
|
||
# ── transcript cleaning (Marcus pattern) ──────────────────────────────────────
|
||
# Gemini's spoken text can CONTAIN our trigger phrases without intending a
|
||
# command — inside a question ("do you want me to move forward?"), a negation
|
||
# ("I'm not turning right"), a hypothetical ("I would be turning right"), a
|
||
# quote, or an echoed [STATE] tag. We drop those whole clauses before matching so
|
||
# only genuine confirmations actuate the robot.
|
||
_BRACKET_RE = re.compile(r"\[[^\]]*\]") # [STATE-DONE] echoes
|
||
_QUOTE_RE = re.compile(r"[\"'«»“”„‟‹›][^\"'«»“”„‟‹›]{0,80}?[\"'«»“”„‟‹›]")
|
||
_SENT_SPLIT_RE = re.compile(r"([.!?؟؛\n]+)") # keep delimiters
|
||
# NOTE: Arabic tokens are whitespace-delimited so we don't match a negation
|
||
# substring inside a real word — e.g. "ما" lives inside "أمام" (forward), "لا"
|
||
# inside many words. \b doesn't help for Arabic (all letters are \w), so we
|
||
# anchor on spaces/string-edges explicitly.
|
||
_NEG_RE = re.compile(
|
||
r"\b(?:not|never|without|cannot|would|could|should|might|instead|"
|
||
r"going to|want to|trying to|rather than)\b|\w+n['’]t\b|"
|
||
r"(?:^|\s)(?:لا|ما|لن|لم|مش|بدون|غير|لست|ليس|بدل)(?:\s|$)")
|
||
|
||
|
||
class MovementDispatcher:
|
||
def __init__(self, loco, instruction_path: Path, state_path: Path):
|
||
self._loco = loco
|
||
self._instruction_path = Path(instruction_path)
|
||
self._state_path = Path(state_path)
|
||
|
||
self._queue: "queue.Queue[Any]" = queue.Queue(maxsize=32)
|
||
self._abort = threading.Event()
|
||
self._worker: Optional[threading.Thread] = None
|
||
self._running = False
|
||
# Hard-stop latch set by a dashboard E-STOP. Drops all voice commands
|
||
# until cleared (by re-enabling Gemini movement). Kept SEPARATE from the
|
||
# movement_enabled file flag so an E-STOP doesn't trigger the spoken
|
||
# "movement disabled" announcement.
|
||
self._estop = False
|
||
|
||
# dedup / cooldown
|
||
self._last_canon = ""
|
||
self._last_at = 0.0
|
||
|
||
# cached enable-flag
|
||
self._enabled_cached = False
|
||
self._enabled_at = 0.0
|
||
|
||
# config (filled by _load)
|
||
self._cooldown = 1.5
|
||
self._max_steps = 8
|
||
self._max_degrees = 360
|
||
self._deg_per_step = 15
|
||
self._fixed_patterns: List[tuple] = [] # (needle_lower, canonical) sorted long→short
|
||
self._parametric: List[tuple] = [] # (compiled_regex, template)
|
||
|
||
self._load()
|
||
|
||
# ── instruction.json ─────────────────────────────────────────────────────
|
||
|
||
def _load(self):
|
||
try:
|
||
data = json.loads(self._instruction_path.read_text(encoding="utf-8"))
|
||
except Exception as exc:
|
||
log.error("could not load %s: %s — dispatcher inert", self._instruction_path, exc)
|
||
data = {}
|
||
self._cooldown = float(data.get("command_cooldown_sec", 1.5))
|
||
self._max_steps = int(data.get("max_steps", 8))
|
||
self._max_degrees = int(data.get("max_degrees", 360))
|
||
self._deg_per_step = max(1, int(data.get("degrees_per_step", 15)))
|
||
|
||
needles: List[tuple] = []
|
||
for spec in (data.get("actions") or {}).values():
|
||
canonical = spec.get("canonical", "")
|
||
phrases = spec.get("bot_phrases", {}) or {}
|
||
for lang_list in phrases.values():
|
||
for p in lang_list:
|
||
if p:
|
||
# English folded to lower; Arabic unaffected by .lower()
|
||
needles.append((p.lower(), canonical))
|
||
# longest needle first so "walking forward" wins over "forward"
|
||
needles.sort(key=lambda t: len(t[0]), reverse=True)
|
||
self._fixed_patterns = needles
|
||
|
||
self._parametric = []
|
||
for pa in (data.get("parametric_actions") or []):
|
||
try:
|
||
self._parametric.append((re.compile(pa["regex"], re.IGNORECASE), pa["canonical"]))
|
||
except re.error as exc:
|
||
log.warning("bad parametric regex %r: %s", pa.get("regex"), exc)
|
||
log.info("instruction.json loaded: %d fixed phrases, %d parametric, cooldown=%.1fs",
|
||
len(self._fixed_patterns), len(self._parametric), self._cooldown)
|
||
|
||
# ── lifecycle ─────────────────────────────────────────────────────────────
|
||
|
||
def start(self):
|
||
if self._running:
|
||
return
|
||
self._running = True
|
||
self._worker = threading.Thread(target=self._worker_loop, daemon=True,
|
||
name="movement-dispatch")
|
||
self._worker.start()
|
||
log.info("movement dispatcher started")
|
||
|
||
def stop(self):
|
||
self._running = False
|
||
self._abort.set()
|
||
try:
|
||
self._queue.put_nowait(_SENTINEL)
|
||
except queue.Full:
|
||
pass
|
||
|
||
def status(self) -> dict:
|
||
return {
|
||
"running": self._running,
|
||
"movement_enabled": self._movement_enabled(force=True),
|
||
"estopped": self._estop,
|
||
"queue_depth": self._queue.qsize(),
|
||
"fixed_phrases": len(self._fixed_patterns),
|
||
"parametric": len(self._parametric),
|
||
}
|
||
|
||
# ── E-STOP latch ──────────────────────────────────────────────────────────
|
||
|
||
def emergency_stop(self):
|
||
"""Latch off after a dashboard E-STOP: abort the in-flight command, drain
|
||
the queue, and refuse new commands until clear_estop(). Does NOT touch the
|
||
movement_enabled file flag (so the Gemini child stays quiet)."""
|
||
self._estop = True
|
||
self._abort.set()
|
||
self._drain()
|
||
log.warning("movement dispatch E-STOP latch set")
|
||
|
||
def clear_estop(self):
|
||
self._estop = False
|
||
|
||
def is_estopped(self) -> bool:
|
||
return self._estop
|
||
|
||
# ── enable gate ───────────────────────────────────────────────────────────
|
||
|
||
def _movement_enabled(self, force: bool = False) -> bool:
|
||
now = time.monotonic()
|
||
if not force and (now - self._enabled_at) < _STATE_CACHE_TTL:
|
||
return self._enabled_cached
|
||
try:
|
||
from Project.Sanad.vision import recognition_state
|
||
self._enabled_cached = bool(recognition_state.read(self._state_path).movement_enabled)
|
||
except Exception:
|
||
self._enabled_cached = False
|
||
self._enabled_at = now
|
||
return self._enabled_cached
|
||
|
||
# ── transcript hook (called from the supervisor reader thread) ────────────
|
||
|
||
def on_bot_text(self, text: str):
|
||
if not text or not self._running or self._estop:
|
||
return
|
||
if not self._movement_enabled():
|
||
return
|
||
cmds = self._match(text)
|
||
if not cmds:
|
||
return
|
||
now = time.monotonic() # monotonic — immune to NTP/wall-clock jumps
|
||
for c in cmds:
|
||
if c == "stop":
|
||
# Safety: preempt anything in flight immediately, then E-STOP.
|
||
self._abort.set()
|
||
self._drain()
|
||
self._enqueue("stop")
|
||
self._last_canon = "stop"
|
||
self._last_at = now
|
||
continue
|
||
# cross-turn cooldown: same canonical not re-fired too soon
|
||
if c == self._last_canon and (now - self._last_at) < self._cooldown:
|
||
continue
|
||
self._last_canon = c
|
||
self._last_at = now
|
||
self._enqueue(c)
|
||
|
||
def _enqueue(self, cmd: str):
|
||
try:
|
||
self._queue.put_nowait(cmd)
|
||
except queue.Full:
|
||
log.warning("motion queue full — dropping %r", cmd)
|
||
|
||
def _drain(self):
|
||
try:
|
||
while True:
|
||
self._queue.get_nowait()
|
||
except queue.Empty:
|
||
pass
|
||
|
||
# ── matcher ───────────────────────────────────────────────────────────────
|
||
|
||
def _clean(self, text: str) -> str:
|
||
"""Drop clauses that are NOT genuine motion confirmations: bracketed
|
||
[STATE] echoes, quoted spans, questions, and negation/hypothetical
|
||
sentences. Only the surviving clauses are matched."""
|
||
t = _BRACKET_RE.sub(" ", text)
|
||
t = _QUOTE_RE.sub(" ", t)
|
||
parts = _SENT_SPLIT_RE.split(t)
|
||
kept: List[str] = []
|
||
i = 0
|
||
while i < len(parts):
|
||
seg = parts[i].strip()
|
||
delim = parts[i + 1] if i + 1 < len(parts) else ""
|
||
is_question = ("?" in delim) or ("؟" in delim)
|
||
if seg and not is_question and not _NEG_RE.search(seg.lower()):
|
||
kept.append(seg)
|
||
i += 2
|
||
return " . ".join(kept)
|
||
|
||
def _match(self, text: str) -> List[str]:
|
||
"""Return canonical commands in spoken order. Parametric (with numbers)
|
||
claim their spans first so a bare phrase doesn't double-fire."""
|
||
low = self._clean(text).lower()
|
||
matches: List[tuple] = [] # (start, canonical)
|
||
claimed: List[tuple] = [] # (start, end) spans already taken
|
||
|
||
def overlaps(s, e):
|
||
return any(s < ce and cs < e for cs, ce in claimed)
|
||
|
||
# 1) parametric first. Claim the span even when the quantity is zero so a
|
||
# mis-heard "0 steps" suppresses the bare phrase underneath (no surprise
|
||
# motion) rather than falling through to a single step.
|
||
for rx, template in self._parametric:
|
||
for m in rx.finditer(low):
|
||
if overlaps(*m.span()):
|
||
continue
|
||
claimed.append(m.span())
|
||
canonical = self._format(template, m.groups())
|
||
if canonical:
|
||
matches.append((m.start(), canonical))
|
||
|
||
# 2) fixed phrases (longest first), skipping claimed spans
|
||
for needle, canonical in self._fixed_patterns:
|
||
start = 0
|
||
while True:
|
||
j = low.find(needle, start)
|
||
if j < 0:
|
||
break
|
||
end = j + len(needle)
|
||
if not overlaps(j, end):
|
||
matches.append((j, canonical))
|
||
claimed.append((j, end))
|
||
start = end
|
||
|
||
matches.sort(key=lambda t: t[0])
|
||
# de-dup consecutive repeats within this single line
|
||
out: List[str] = []
|
||
for _, c in matches:
|
||
if not out or out[-1] != c:
|
||
out.append(c)
|
||
return out
|
||
|
||
@staticmethod
|
||
def _format(template: str, groups) -> str:
|
||
out = template
|
||
for i, g in enumerate(groups, start=1):
|
||
out = out.replace(f"${i}", str(g))
|
||
# reject zero-quantity motions ("walk 0 steps")
|
||
nums = re.findall(r"\d+", out)
|
||
if nums and all(int(n) == 0 for n in nums):
|
||
return ""
|
||
return out
|
||
|
||
# ── worker ────────────────────────────────────────────────────────────────
|
||
|
||
def _worker_loop(self):
|
||
while self._running:
|
||
cmd = self._queue.get()
|
||
if cmd is _SENTINEL:
|
||
return
|
||
if cmd != "stop":
|
||
self._abort.clear()
|
||
if self._estop:
|
||
continue # E-STOP latched — drop everything
|
||
# force a fresh read — don't let the 0.5s cache execute a command
|
||
# after the operator just toggled movement off.
|
||
if cmd != "stop" and not self._movement_enabled(force=True):
|
||
continue # toggled off while queued — drop
|
||
# Never step while Nav2 owns the legs (autonomous goal in progress).
|
||
# Two stacks driving at once is the exact hazard _arbiter guards.
|
||
# STOP always passes through (safety). Read-only check — manual loco
|
||
# uses acquire_loco; the discrete-step voice path must only YIELD.
|
||
if cmd != "stop" and self._nav_holds_legs():
|
||
log.info("voice movement dropped — Nav2 owns the legs (%r)", cmd)
|
||
continue
|
||
try:
|
||
self._execute(cmd)
|
||
except Exception:
|
||
log.exception("execute %r failed", cmd)
|
||
|
||
def _execute(self, canonical: str):
|
||
c = canonical.lower().strip()
|
||
if c == "stop":
|
||
log.info("voice → STOP")
|
||
self._loco.estop()
|
||
return
|
||
|
||
m = re.match(r"walk (forward|backward) (\d+) steps?$", c)
|
||
if m:
|
||
direction = "forward" if m.group(1) == "forward" else "backward"
|
||
n = min(int(m.group(2)), self._max_steps)
|
||
log.info("voice → walk %s %d steps", direction, n)
|
||
self._repeat_step(direction, n)
|
||
return
|
||
|
||
m = re.match(r"turn (right|left) (\d+) degrees?$", c)
|
||
if m:
|
||
direction = "rotate_right" if m.group(1) == "right" else "rotate_left"
|
||
deg = min(int(m.group(2)), self._max_degrees)
|
||
n = max(1, round(deg / self._deg_per_step))
|
||
log.info("voice → turn %s %d° (~%d steps)", m.group(1), deg, n)
|
||
self._repeat_step(direction, n)
|
||
return
|
||
|
||
direction = _FIXED_STEP.get(c)
|
||
if direction:
|
||
log.info("voice → %s", c)
|
||
self._loco.step(direction)
|
||
return
|
||
log.debug("no loco mapping for canonical %r", c)
|
||
|
||
@staticmethod
|
||
def _nav_holds_legs() -> bool:
|
||
"""True if Nav2 currently owns the legs (in-process arbiter). Lazy
|
||
import so a missing/absent dashboard package never breaks voice."""
|
||
try:
|
||
from Project.Sanad.dashboard.routes import _arbiter
|
||
return _arbiter.nav_active()
|
||
except Exception:
|
||
return False
|
||
|
||
def _repeat_step(self, direction: str, n: int):
|
||
for _ in range(max(1, n)):
|
||
if (self._abort.is_set() or self._estop
|
||
or not self._movement_enabled(force=True)
|
||
or self._nav_holds_legs()):
|
||
log.info("voice multi-step aborted")
|
||
break
|
||
self._loco.step(direction)
|