diff --git a/.claude/settings.json b/.claude/settings.json new file mode 100644 index 0000000..49630dd --- /dev/null +++ b/.claude/settings.json @@ -0,0 +1,7 @@ +{ + "permissions": { + "allow": [ + "Bash(node -e ' *)" + ] + } +} diff --git a/G1_Controller/__init__.py b/G1_Controller/__init__.py new file mode 100644 index 0000000..bee07a0 --- /dev/null +++ b/G1_Controller/__init__.py @@ -0,0 +1,12 @@ +"""G1_Controller — manual dashboard locomotion control (N2 Phase 1). + +`LocoController` wraps the Unitree `LocoClient` + `MotionSwitcherClient` for +operator-driven walking, postures and a discrete step pad. It reuses the arm +controller's single process-wide DDS init (one `ChannelFactoryInitialize`) and +is gated behind an in-memory "Enable movement" arm flag that defaults OFF every +boot. See dashboard/routes/controller.py for the REST surface. +""" + +from Project.Sanad.G1_Controller.loco_controller import LocoController + +__all__ = ["LocoController"] diff --git a/G1_Controller/loco_controller.py b/G1_Controller/loco_controller.py new file mode 100644 index 0000000..f2746db --- /dev/null +++ b/G1_Controller/loco_controller.py @@ -0,0 +1,567 @@ +"""LocoController — manual G1 locomotion via the Unitree LocoClient (N2 Phase 1). + +Ported from the proven scripts in G1_Lootah/Controller (g1_mode_controller.py, +keyboard_controller.py, hanger_boot_sequence.py). Design notes: + +* **One DDS init per process.** The arm controller owns the single + `ChannelFactoryInitialize(0, nic)` (motion/arm_controller.py). This class + NEVER initialises DDS — it lazily builds its `LocoClient` / + `MotionSwitcherClient` only after `arm._initialized` is True. +* **Default DISARMED.** `_armed` starts False every boot and gates every WRITE + method. Reads (status / fsm / joints), E-STOP and disarm are ALWAYS allowed. +* **StopMove watchdog.** Continuous `Move(..., True)` never self-terminates, so a + daemon thread StopMoves if no `move()` refresh arrives within + `watchdog_timeout_sec`. The frontend re-sends setpoints at ~10 Hz, so a tab + close / network drop trips the watchdog within the timeout. +* **Velocity caps.** Symmetric clamp on vx/vy/vyaw — Walk 0.6, Run 1.2. +* **Allow-anytime-warn.** move/step never hard-block on FSM; if not walk-ready + they still execute but return a `warning`. +* **Sim fallback.** When `unitree_sdk2py` is absent (workstation), every write + returns `{"simulated": True}` (never raises) so the whole UI is testable. + +SDK facts confirmed from source — do not "fix" them: +* `LocoClient.Move(vx, vy, vyaw, True)` — the continuous-mode kwarg is misspelled + `continous_move` (one n); we pass it POSITIONALLY to avoid a TypeError. +* `LocoClient` has NO StandUp()/Squat() — use SetFsmId(4)/SetFsmId(2). +* FSM id / mode are read via the private RPC `bot._Call(7001/7002, "{}")`. +""" + +from __future__ import annotations + +import json +import threading +import time +from typing import Any, Optional + +from Project.Sanad.core.config_loader import section as _cfg_section +from Project.Sanad.core.logger import get_logger + +log = get_logger("loco_controller") + +# -- SDK import (optional) ----------------------------------------------------- +try: + from unitree_sdk2py.g1.loco.g1_loco_client import LocoClient + from unitree_sdk2py.comm.motion_switcher.motion_switcher_client import ( + MotionSwitcherClient, + ) + _HAS_SDK = True +except ImportError: + LocoClient = None + MotionSwitcherClient = None + _HAS_SDK = False + log.warning("Unitree SDK not available — LocoController in simulation mode") + +# LocoClient general RPC api-ids for FSM read-back (stable across SDK builds). +ROBOT_API_ID_LOCO_GET_FSM_ID = 7001 +ROBOT_API_ID_LOCO_GET_FSM_MODE = 7002 + +# G1 29-DoF joint names for indices 12-28 (0-11 legs, 12-14 waist, 15-21 left +# arm, 22-28 right arm). Used by the Diagnostics joint read-out. +JOINT_NAMES = { + 12: "WAIST_YAW", 13: "WAIST_ROLL", 14: "WAIST_PITCH", + 15: "L_SHOULDER_PITCH", 16: "L_SHOULDER_ROLL", 17: "L_SHOULDER_YAW", + 18: "L_ELBOW", 19: "L_WRIST_ROLL", 20: "L_WRIST_PITCH", 21: "L_WRIST_YAW", + 22: "R_SHOULDER_PITCH", 23: "R_SHOULDER_ROLL", 24: "R_SHOULDER_YAW", + 25: "R_ELBOW", 26: "R_WRIST_ROLL", 27: "R_WRIST_PITCH", 28: "R_WRIST_YAW", +} + +# Discrete step pad — (vx, vy, vyaw) sign per direction; magnitude is +# step_speed_frac * cap_walk (a gentle single step). +_STEP_DIRS = { + "forward": (1.0, 0.0, 0.0), + "backward": (-1.0, 0.0, 0.0), + "slide_left": (0.0, 1.0, 0.0), + "slide_right": (0.0, -1.0, 0.0), + "rotate_left": (0.0, 0.0, 1.0), + "rotate_right": (0.0, 0.0, -1.0), +} + +_POSTURES = ( + "zero_torque", "damp", "stand_up", "squat", "sit", + "low_stand", "high_stand", "lie_to_stand", +) + + +class LocoController: + """Thread-safe manual locomotion control with a simulation fallback.""" + + def __init__(self, arm=None): + self._arm = arm # shared ArmController (owns the ONE DDS init) + self._bot = None # LocoClient (lazy) + self._msc = None # MotionSwitcherClient (lazy) + self._lc_ready = False + self._lock = threading.RLock() # serialise all loco client WRITE calls + self._armed = False # in-memory MANUAL gate — OFF every boot + + self._cur_v = (0.0, 0.0, 0.0) # last commanded (vx, vy, vyaw) + self._teleop_active = False + self._last_msc_mode: Optional[str] = None + + # watchdog + self._last_move_ts = 0.0 + self._wd_thread: Optional[threading.Thread] = None + self._wd_stop = threading.Event() + self._wd_stop.set() # not running until armed + # Monotonic stop-generation counter, bumped under _lock by + # estop/stop/disarm. move()/step()/prep_mode() capture it at start and + # bail the instant it changes — so E-STOP preempts an in-flight motion + # immediately AND can never be silently "un-cancelled" by a concurrent + # command (a lock-free Event clear() could; an int compare under the + # lock cannot). + self._stop_gen = 0 + # Serializes the discrete blocking operations (step/prep_mode) so two + # can't overlap and interleave Move commands. Continuous teleop move() + # is intentionally NOT guarded by this. + self._discrete_busy = False + + cfg = _cfg_section("motion", "loco_controller") + self._cap_walk = float(cfg.get("cap_walk", 0.6)) + self._cap_run = float(cfg.get("cap_run", 1.2)) + self._lin_step = float(cfg.get("lin_step", 0.05)) + self._ang_step = float(cfg.get("ang_step", 0.2)) + self._wd_timeout = float(cfg.get("watchdog_timeout_sec", 0.5)) + self._block_window = float(cfg.get("arm_block_window_sec", 1.5)) + self._step_dur = float(cfg.get("step_duration_sec", 0.6)) + self._step_frac = float(cfg.get("step_speed_frac", 0.5)) + self._loco_timeout = float(cfg.get("loco_timeout_sec", 10.0)) + self._msc_timeout = float(cfg.get("msc_timeout_sec", 5.0)) + + # ── client lifecycle ───────────────────────────────────────────────────── + + def _ensure_client(self) -> bool: + """Lazily build LocoClient + MotionSwitcherClient. Returns readiness. + + Never initialises DDS — requires the shared arm to have already run the + single ChannelFactoryInitialize. + """ + if not _HAS_SDK: + return False + if self._lc_ready: + return True + if self._arm is None or not getattr(self._arm, "_initialized", False): + return False + with self._lock: + if self._lc_ready: + return True + try: + bot = LocoClient() + bot.SetTimeout(self._loco_timeout) + bot.Init() + msc = MotionSwitcherClient() + msc.SetTimeout(self._msc_timeout) + msc.Init() + self._bot = bot + self._msc = msc + self._lc_ready = True + log.info("LocoClient + MotionSwitcherClient ready") + except Exception as exc: + log.error("LocoClient init failed: %s", exc) + self._lc_ready = False + return self._lc_ready + + def _safe_call(self, name: str, fn, *a, **kw): + try: + return True, fn(*a, **kw) + except Exception as exc: + log.error("%s failed: %s", name, exc) + return False, None + + def _rpc_get_int(self, api_id: int): + bot = self._bot + if bot is None: + return None + try: + code, data = bot._Call(api_id, "{}") + if code == 0 and data: + return json.loads(data).get("data") + except Exception: + pass + return None + + @staticmethod + def _clamp(v: float, cap: float) -> float: + return max(-cap, min(cap, float(v))) + + # ── FSM / readiness ────────────────────────────────────────────────────── + + def fsm_id(self): + return self._rpc_get_int(ROBOT_API_ID_LOCO_GET_FSM_ID) + + def fsm_mode(self): + return self._rpc_get_int(ROBOT_API_ID_LOCO_GET_FSM_MODE) + + def _walk_ready_warning(self) -> Optional[str]: + """allow-anytime-warn: None when ready, else a human message.""" + if not self._lc_ready: + return None + fid = self.fsm_id() + fmode = self.fsm_mode() + if fid == 200 and fmode not in (None, 2): + return None + return (f"Robot not in walk-ready FSM (id={fid}, mode={fmode}). " + f"Command sent anyway.") + + # ── arm flag + watchdog ────────────────────────────────────────────────── + + def is_armed(self) -> bool: + return self._armed + + def movement_active(self) -> bool: + """True when the robot may be walking: manual armed, teleop active, OR a + move/step issued within the block window. Used as the arm's motion-block + predicate so the arm never replays while the robot is (or just was) + moving — regardless of whether the MANUAL gate or the GEMINI gate + (Phase 3 voice dispatch, which calls move/step directly) triggered it.""" + if self._armed or self._teleop_active: + return True + return (time.monotonic() - self._last_move_ts) < self._block_window + + def arm_movement(self) -> dict: + """Unlock manual control. Cancels any in-flight arm motion first so the + arm and locomotion are never active simultaneously (movement wins).""" + try: + if self._arm is not None and getattr(self._arm, "is_busy", False): + log.info("arming movement — cancelling in-flight arm motion") + self._arm.cancel() + except Exception: + log.exception("arm.cancel() on arm_movement failed") + with self._lock: + self._armed = True + self._start_watchdog() + log.info("movement ARMED") + return {"ok": True, "armed": True} + + def disarm_movement(self) -> dict: + with self._lock: + self._stop_gen += 1 # break any in-flight step/prep/move + self._armed = False + self._teleop_active = False + self._wd_stop.set() + try: + self._raw_stop() + except Exception: + log.exception("StopMove on disarm failed") + log.info("movement DISARMED") + return {"ok": True, "armed": False} + + def _start_watchdog(self): + self._wd_stop.clear() + if self._wd_thread is None or not self._wd_thread.is_alive(): + self._wd_thread = threading.Thread( + target=self._watchdog_loop, daemon=True, name="loco-watchdog") + self._wd_thread.start() + + def _watchdog_loop(self): + period = max(0.02, min(0.1, self._wd_timeout / 2.0)) + while not self._wd_stop.is_set(): + fire = False + # Read-and-decide under the lock (atomic check-then-act); the actual + # StopMove runs after release so the critical section stays tiny. + with self._lock: + if self._teleop_active and (time.monotonic() - self._last_move_ts) > self._wd_timeout: + self._teleop_active = False + fire = True + if fire: + log.warning("watchdog: teleop setpoint stale (>%.2fs) — StopMove", + self._wd_timeout) + try: + self._raw_stop() + except Exception: + log.exception("watchdog StopMove failed") + self._wd_stop.wait(period) + + def _raw_stop(self) -> bool: + """Issue StopMove if the client is up; no-op in sim. Lock-light.""" + if not self._lc_ready or self._bot is None: + return False + with self._lock: + ok, _ = self._safe_call("StopMove", self._bot.StopMove) + return ok + + # ── movement ───────────────────────────────────────────────────────────── + + def move(self, vx: float, vy: float, vyaw: float, run: bool = False) -> dict: + cap = self._cap_run if run else self._cap_walk + cvx, cvy, cvyaw = self._clamp(vx, cap), self._clamp(vy, cap), self._clamp(vyaw, cap) + capped = (cvx, cvy, cvyaw) != (float(vx), float(vy), float(vyaw)) + warning = self._walk_ready_warning() + sent = {"vx": cvx, "vy": cvy, "vyaw": cvyaw} + + with self._lock: + my_gen = self._stop_gen # capture under lock + + if not self._ensure_client(): + with self._lock: # sim: record intent for UI/watchdog + self._cur_v = (cvx, cvy, cvyaw) + self._last_move_ts = time.monotonic() + self._teleop_active = True + self._start_watchdog() + return {"ok": True, "sent": sent, "capped": capped, + "warning": warning, "simulated": True} + with self._lock: + # If an E-STOP / stop / disarm landed since we captured my_gen, do NOT + # (re)command velocity — and do NOT stamp the motion flags (so a + # cancelled tick doesn't extend the arm-block window). + if self._stop_gen != my_gen: + return {"ok": False, "cancelled": True, "sent": sent, + "capped": capped, "warning": warning, "simulated": False} + self._cur_v = (cvx, cvy, cvyaw) + self._last_move_ts = time.monotonic() + self._teleop_active = True + self._safe_call("SetBalanceMode", self._bot.SetBalanceMode, 1) + ok, _ = self._safe_call("Move", self._bot.Move, cvx, cvy, cvyaw, True) + self._start_watchdog() + return {"ok": bool(ok), "sent": sent, "capped": capped, + "warning": warning, "simulated": False} + + def stop_move(self) -> dict: + """Halt translation/rotation. Allowed even when disarmed.""" + with self._lock: + self._stop_gen += 1 + self._teleop_active = False + if not self._ensure_client(): + return {"ok": True, "simulated": True} + ok = self._raw_stop() + return {"ok": bool(ok), "simulated": False} + + def estop(self) -> dict: + """Emergency stop = StopMove only (no Damp / FSM change → keeps posture). + ALWAYS allowed, even disarmed and in sim. Bumps the stop generation so any + in-flight move()/step()/prep_mode() bails immediately (no lock wait).""" + with self._lock: + self._stop_gen += 1 + self._teleop_active = False + self._cur_v = (0.0, 0.0, 0.0) + if not self._ensure_client(): + log.warning("E-STOP (sim)") + return {"ok": True, "simulated": True} + ok = self._raw_stop() + log.warning("E-STOP — StopMove issued") + return {"ok": bool(ok), "simulated": False} + + def step(self, direction: str) -> dict: + """Discrete one-step pad: Move for step_duration then StopMove. + Blocking (~step_duration); call via asyncio.to_thread from the route. + + The sleep loop does NOT hold self._lock, so E-STOP / StopMove (which take + the lock briefly) preempt it immediately; the loop also bails the moment + the stop generation changes.""" + if direction not in _STEP_DIRS: + return {"ok": False, "reason": f"unknown direction: {direction}"} + sx, sy, syaw = _STEP_DIRS[direction] + k = self._cap_walk * self._step_frac + vx, vy, vyaw = sx * k, sy * k, syaw * k + warning = self._walk_ready_warning() + with self._lock: + if self._discrete_busy: + return {"ok": False, "dir": direction, "reason": "busy", + "warning": warning, "simulated": not self._lc_ready} + self._discrete_busy = True + my_gen = self._stop_gen + self._last_move_ts = time.monotonic() + self._teleop_active = True + self._start_watchdog() + if not self._ensure_client(): + with self._lock: + self._teleop_active = False + self._discrete_busy = False + return {"ok": True, "dir": direction, "warning": warning, "simulated": True} + try: + with self._lock: + if self._stop_gen != my_gen: # stopped before we began + return {"ok": False, "dir": direction, "cancelled": True, + "warning": warning, "simulated": False} + self._safe_call("SetBalanceMode", self._bot.SetBalanceMode, 1) + self._safe_call("Move", self._bot.Move, vx, vy, vyaw, True) + t_end = time.monotonic() + self._step_dur + while time.monotonic() < t_end: + if self._stop_gen != my_gen: + break + with self._lock: + self._last_move_ts = time.monotonic() # keep watchdog fed + time.sleep(0.05) + finally: + with self._lock: + self._safe_call("StopMove", self._bot.StopMove) + self._teleop_active = False + self._discrete_busy = False + return {"ok": True, "dir": direction, "warning": warning, "simulated": False} + + # ── postures / modes ───────────────────────────────────────────────────── + + def prep_mode(self) -> dict: + """PREP — StopMove → Damp → StandUp(FSM4) → height ramp → BalanceStand(0). + Exact order from g1_mode_controller.prep_mode, minus the blocking input(). + Blocking (~1s); call via asyncio.to_thread.""" + if not self._ensure_client(): + return {"ok": True, "mode": "prep", "simulated": True} + with self._lock: + if self._discrete_busy: + return {"ok": False, "mode": "prep", "reason": "busy", "simulated": False} + self._discrete_busy = True + my_gen = self._stop_gen + self._safe_call("StopMove", self._bot.StopMove) + self._safe_call("Damp", self._bot.Damp) + self._safe_call("SetFsmId(4)", self._bot.SetFsmId, 4) + try: + # Height ramp OUTSIDE the lock so E-STOP can preempt at any time. + h = 0.02 + while h <= 0.5 + 1e-9: + if self._stop_gen != my_gen: + log.warning("PREP cancelled (E-STOP)") + return {"ok": False, "mode": "prep", "cancelled": True, "simulated": False} + with self._lock: + self._safe_call("SetStandHeight", self._bot.SetStandHeight, round(h, 3)) + time.sleep(0.03) + h += 0.02 + with self._lock: + self._safe_call("BalanceStand", self._bot.BalanceStand, 0) + self._safe_call("SetStandHeight", self._bot.SetStandHeight, 0.22) + finally: + with self._lock: + self._discrete_busy = False + log.info("PREP complete") + return {"ok": True, "mode": "prep", "simulated": False} + + def ready_start_mode(self) -> dict: + """READY = PREP then Start (FSM 200 / balance engaged).""" + self.prep_mode() + if not self._ensure_client(): + return {"ok": True, "mode": "ready", "simulated": True} + with self._lock: + if hasattr(self._bot, "Start"): + ok, _ = self._safe_call("Start", self._bot.Start) + else: + ok, _ = self._safe_call("SetFsmId(200)", self._bot.SetFsmId, 200) + log.info("READY/START complete") + return {"ok": bool(ok), "mode": "ready", "simulated": False} + + def posture(self, name: str) -> dict: + if name not in _POSTURES: + return {"ok": False, "reason": f"unknown posture: {name}"} + if not self._ensure_client(): + return {"ok": True, "posture": name, "simulated": True} + bot = self._bot + with self._lock: + if name == "zero_torque": + ok, _ = self._safe_call("ZeroTorque", bot.ZeroTorque) + elif name == "damp": + ok, _ = self._safe_call("Damp", bot.Damp) + elif name == "stand_up": + ok, _ = self._safe_call("SetFsmId(4)", bot.SetFsmId, 4) + elif name == "squat": + ok, _ = self._safe_call("SetFsmId(2)", bot.SetFsmId, 2) + elif name == "sit": + ok, _ = self._safe_call("Sit", bot.Sit) + elif name == "low_stand": + ok, _ = self._safe_call("LowStand", bot.LowStand) + elif name == "high_stand": + ok, _ = self._safe_call("HighStand", bot.HighStand) + elif name == "lie_to_stand": + if hasattr(bot, "Lie2StandUp"): + ok, _ = self._safe_call("Lie2StandUp", bot.Lie2StandUp) + else: + ok, _ = self._safe_call("SetFsmId(702)", bot.SetFsmId, 702) + else: # unreachable (guarded above) + ok = False + return {"ok": bool(ok), "posture": name, "simulated": False} + + def set_balance_mode(self, mode: int) -> dict: + if not self._ensure_client(): + return {"ok": True, "balance_mode": int(mode), "simulated": True} + with self._lock: + ok, _ = self._safe_call("SetBalanceMode", self._bot.SetBalanceMode, int(mode)) + return {"ok": bool(ok), "balance_mode": int(mode), "simulated": False} + + def set_stand_height(self, h: float) -> dict: + if not self._ensure_client(): + return {"ok": True, "height": float(h), "simulated": True} + with self._lock: + ok, _ = self._safe_call("SetStandHeight", self._bot.SetStandHeight, float(h)) + return {"ok": bool(ok), "height": float(h), "simulated": False} + + # ── MotionSwitcher ─────────────────────────────────────────────────────── + + def msc_check(self) -> dict: + if not self._ensure_client() or self._msc is None: + return {"mode_name": None, "simulated": not self._lc_ready} + try: + ret = self._msc.CheckMode() + name = None + if isinstance(ret, tuple) and len(ret) >= 2 and isinstance(ret[1], dict): + name = ret[1].get("name") + elif isinstance(ret, dict): + name = ret.get("name") + self._last_msc_mode = name + return {"mode_name": name} + except Exception as exc: + log.error("msc_check failed: %s", exc) + return {"mode_name": None} + + def msc_select_ai(self) -> dict: + if not self._ensure_client() or self._msc is None: + return {"ok": True, "simulated": True} + with self._lock: + ok, _ = self._safe_call("SelectMode(ai)", self._msc.SelectMode, "ai") + return {"ok": bool(ok), "simulated": False} + + def msc_release(self) -> dict: + if not self._ensure_client() or self._msc is None: + return {"ok": True, "simulated": True} + with self._lock: + ok, _ = self._safe_call("ReleaseMode", self._msc.ReleaseMode) + return {"ok": bool(ok), "simulated": False} + + def reconnect(self) -> dict: + """Drop and rebuild Loco + MSC clients (does NOT re-init the DDS factory).""" + with self._lock: + self._bot = None + self._msc = None + self._lc_ready = False + ok = self._ensure_client() + return {"ok": bool(ok), "lc_ready": self._lc_ready} + + # ── reads ──────────────────────────────────────────────────────────────── + + def joints(self) -> dict: + q: list = [] + try: + if self._arm is not None: + q = self._arm.get_current_q() + except Exception: + q = [] + out = [] + for idx in range(12, 29): + val = q[idx] if idx < len(q) else 0.0 + out.append({"idx": idx, "name": JOINT_NAMES.get(idx, f"motor_{idx}"), + "q": float(val)}) + return {"joints": out} + + def status(self) -> dict: + # Polling /status lazily brings up the client once arm DDS is ready. + self._ensure_client() + fid = self.fsm_id() if self._lc_ready else None + fmode = self.fsm_mode() if self._lc_ready else None + walk_ready = bool(self._lc_ready and fid == 200 and fmode not in (None, 2)) + return { + "sdk_available": _HAS_SDK, + "lc_ready": self._lc_ready, + "armed": self._armed, + "fsm_id": fid, + "fsm_mode": fmode, + "walk_ready": walk_ready, + "msc_mode": self._last_msc_mode, + "teleop_active": self._teleop_active, + "last_velocity": {"vx": self._cur_v[0], "vy": self._cur_v[1], "vyaw": self._cur_v[2]}, + "caps": {"walk": self._cap_walk, "run": self._cap_run}, + "arm_initialized": bool(self._arm is not None and getattr(self._arm, "_initialized", False)), + } + + # ── shutdown helper ────────────────────────────────────────────────────── + + def shutdown(self): + """Best-effort StopMove + disarm for process shutdown.""" + try: + self.estop() + finally: + self.disarm_movement() diff --git a/config/core_config.json b/config/core_config.json index be31af4..177616b 100644 --- a/config/core_config.json +++ b/config/core_config.json @@ -41,7 +41,7 @@ "model_ws_uri": "wss://generativelanguage.googleapis.com/ws/google.ai.generativelanguage.v1alpha.GenerativeService.BidiGenerateContent", "voice_name": "Charon", "ws_timeout_sec": 30, - "default_system_prompt": "You are Sanad (Bousandah), a wise and friendly Emirati assistant. Speak strictly in the UAE dialect (Khaleeji). Be helpful, concise, and use local greetings like 'Marhaba' and 'Ya Khoy'." + "default_system_prompt": "You are Bousandah, a wise and friendly Emirati assistant. Speak strictly in the UAE dialect (Khaleeji). Be helpful, concise, and use local greetings like 'Marhaba' and 'Ya Khoy'." }, "g1_hardware": { @@ -53,7 +53,7 @@ "script_files": { "_comment": "Filenames (under scripts/) used across voice + dashboard", - "persona": "sanad_v2", + "persona": "sanad_script.txt", "rules": "sanad_rule.txt", "arm_phrases": "sanad_arm.txt" }, diff --git a/config/motion_config.json b/config/motion_config.json index bcc3336..9070df1 100644 --- a/config/motion_config.json +++ b/config/motion_config.json @@ -13,6 +13,20 @@ "jsonl_id_start": 100 }, + "loco_controller": { + "_comment": "G1_Controller/loco_controller.py — manual locomotion. NIC is shared from the arm's DDS init (config core.dds / SANAD_DDS_INTERFACE), not set here.", + "cap_walk": 0.6, + "cap_run": 1.2, + "lin_step": 0.05, + "ang_step": 0.2, + "watchdog_timeout_sec": 0.5, + "arm_block_window_sec": 1.5, + "step_duration_sec": 0.6, + "step_speed_frac": 0.5, + "loco_timeout_sec": 10.0, + "msc_timeout_sec": 5.0 + }, + "macro_player": { "_comment": "motion/macro_player.py — JSONL playback", "ramp_in_steps": 60, diff --git a/dashboard/app.py b/dashboard/app.py index 5de8495..8261aab 100644 --- a/dashboard/app.py +++ b/dashboard/app.py @@ -51,9 +51,12 @@ _REST_ROUTES: list[tuple[str, str, str]] = [ ("live_subprocess", "/api/live-subprocess", "live-subprocess"), ("typed_replay", "/api/typed-replay", "typed-replay"), ("recognition", "/api/recognition", "recognition"), + ("zones", "/api/zones", "zones"), + ("temp_monitor", "/api/temp", "temperature"), + ("controller", "/api/controller", "controller"), ] -_WS_ROUTES: list[str] = ["log_stream"] +_WS_ROUTES: list[str] = ["log_stream", "motor_temps", "terminal"] _loaded_routes: list[str] = [] _failed_routes: dict[str, str] = {} diff --git a/dashboard/routes/audio_control.py b/dashboard/routes/audio_control.py index 3fc21a2..6ed28f1 100644 --- a/dashboard/routes/audio_control.py +++ b/dashboard/routes/audio_control.py @@ -473,3 +473,450 @@ async def apply_audio(): pass return result return await asyncio.to_thread(_do) + + +# ─────────────────────── Reset endpoints (Pulse + USB) ─────────────────────── +# +# Two distinct recovery paths for the dashboard's audio panel: +# +# POST /api/audio/reset — SOFT: restart pulseaudio / pipewire-pulse. +# Fixes Pulse-side state (stuck profile, lost default sink, crashed +# module). Cannot recover a kernel-side missing USB capture descriptor +# — snd-usb-audio parses those at probe time and Pulse can't influence +# that. Use for "devices look weird" failures. +# +# POST /api/audio/usb-reset — HARD: unbind+rebind snd-usb-audio scoped +# to the Anker VID:PID. Forces snd-usb-audio to re-parse UAC1 +# descriptors → input profile reappears even after the firmware/USB +# handshake dropped it. Use for "Anker mic missing from pactl" — the +# symptom soft-reset cannot fix. +# +# Both gate with module-level locks (no concurrent reset), refuse while Live +# Gemini is running or a record is mid-playback, and return structured +# before/after diagnostics so the dashboard can show meaningful toasts. + +_RESET_LOCK = threading.Lock() +_USB_RESET_LOCK = threading.Lock() + +# Anker PowerConf A3321 — used both for VID:PID matching in sysfs and for +# logging. Change here if you add support for a different USB conference +# device (Hollyland etc). +_USB_RESET_TARGETS = ( + {"vid": "291a", "pid": "3301", "label": "Anker PowerConf"}, +) + + +def _refuse_if_busy() -> None: + """Raise HTTPException(409) if Live Gemini is active or a record is playing. + + Used by both reset endpoints — a userspace audio restart mid-stream + leaves the active session in a broken state (PortAudio handle pointing + at a dead Pulse, in-flight write() raises, etc.). Cheaper to refuse + than to recover. + """ + try: + from Project.Sanad.main import live_sub + except Exception: + live_sub = None + if live_sub is not None: + try: + st = live_sub.status() or {} + except Exception: + st = {} + state = (st.get("state") or "").lower() + if st.get("running") or state not in ("", "stopped", "error"): + raise HTTPException( + 409, f"Stop Live Gemini before resetting audio (state={state or '?'}).", + ) + + try: + from Project.Sanad.main import audio_mgr + except Exception: + audio_mgr = None + if audio_mgr is not None and hasattr(audio_mgr, "playback_status"): + try: + ps = audio_mgr.playback_status() or {} + if ps.get("playing"): + raise HTTPException( + 409, "Stop the active playback before resetting audio.", + ) + except HTTPException: + raise + except Exception: + pass + + +def _detect_pa_flavour() -> str: + """Return 'pipewire' if pipewire-pulse is the active daemon, else 'pulse'.""" + try: + r = subprocess.run( + ["pgrep", "-x", "pipewire-pulse"], + check=False, capture_output=True, text=True, timeout=1.0, + ) + if r.returncode == 0 and (r.stdout or "").strip(): + return "pipewire" + except (FileNotFoundError, subprocess.SubprocessError): + pass + return "pulse" + + +def _kill_audio_daemon(flavour: str) -> dict: + """Issue the restart command for the detected daemon. Non-zero exit is a + soft warning (some installs return 1 when there's no daemon to kill).""" + if flavour == "pipewire": + cmd = ["systemctl", "--user", "restart", "pipewire-pulse.service"] + else: + cmd = ["pulseaudio", "-k"] + try: + r = subprocess.run(cmd, check=False, capture_output=True, + text=True, timeout=5.0) + info = {"cmd": " ".join(cmd), "returncode": r.returncode, + "stderr": (r.stderr or "").strip()[:300]} + if r.returncode != 0: + log.warning("audio reset: %s exited %d (%s)", + cmd[0], r.returncode, info["stderr"]) + return info + except FileNotFoundError as exc: + return {"cmd": " ".join(cmd), "returncode": -1, + "stderr": f"binary missing: {exc}"} + except subprocess.TimeoutExpired: + return {"cmd": " ".join(cmd), "returncode": -1, + "stderr": "timeout (>5s)"} + + +def _wait_for_pactl(deadline_s: float = 5.0, interval_s: float = 0.2) -> bool: + """Poll `pactl info` until it returns 0 or the deadline expires.""" + import time as _time + end = _time.monotonic() + deadline_s + while _time.monotonic() < end: + if ad.pactl_available(): + return True + _time.sleep(interval_s) + return False + + +@router.post("/reset") +async def reset_audio_subsystem(): + """SOFT reset — restart pulseaudio/pipewire-pulse and re-resolve devices. + + Use when devices look stuck, pactl is unavailable, or the wrong sink + is being selected. **Does NOT recover a kernel-side missing USB capture + descriptor** — for that symptom use /api/audio/usb-reset. + """ + if os.geteuid() == 0: + raise HTTPException( + 403, "Refusing to reset audio as root — Sanad must run as the " + "unitree user so the per-user PulseAudio session is reachable.", + ) + if not _RESET_LOCK.acquire(blocking=False): + raise HTTPException(429, "Reset already in progress.") + try: + _refuse_if_busy() + log.info( + "audio reset requested (uid=%s PULSE_RUNTIME_PATH=%s XDG_RUNTIME_DIR=%s)", + os.geteuid(), + os.environ.get("PULSE_RUNTIME_PATH") or "-", + os.environ.get("XDG_RUNTIME_DIR") or "-", + ) + try: + from Project.Sanad.main import audio_mgr + except Exception: + audio_mgr = None + + def _do() -> dict: + before = {"pactl_available": ad.pactl_available(), + "selection": ad.current_selection()} + + # Quiesce AudioManager so the next play_wav rebinds cleanly. + pya_closed = False + if audio_mgr is not None: + play_lock = getattr(audio_mgr, "play_lock", None) + acquired = False + if play_lock is not None: + acquired = play_lock.acquire(timeout=2.0) + try: + try: + audio_mgr.close() + pya_closed = True + except Exception as exc: + log.warning("audio reset: audio_mgr.close failed: %s", exc) + finally: + if acquired and play_lock is not None: + play_lock.release() + + flavour = _detect_pa_flavour() + kill_info = _kill_audio_daemon(flavour) + came_back = _wait_for_pactl(deadline_s=5.0) + if not came_back and flavour == "pulse": + # autospawn may be disabled — try an explicit start. + try: + subprocess.run(["pulseaudio", "--start"], check=False, + capture_output=True, text=True, timeout=3.0) + except (FileNotFoundError, subprocess.SubprocessError) as exc: + log.warning("audio reset: pulseaudio --start failed: %s", exc) + came_back = _wait_for_pactl(deadline_s=2.0) + if not came_back: + raise HTTPException(500, { + "error": "audio daemon did not return within ~7s", + "flavour": flavour, "kill": kill_info, + }) + + apply_result: dict = {} + try: + apply_result = ad.apply_current_selection() or {} + except Exception as exc: + log.warning("audio reset: apply_current_selection failed: %s", exc) + apply_result = {"error": str(exc)} + + if audio_mgr is not None: + try: + import pyaudio + audio_mgr.pya = pyaudio.PyAudio() + audio_mgr.refresh_devices() + except Exception as exc: + log.error("audio reset: PyAudio re-init failed: %s", exc) + raise HTTPException( + 500, f"PortAudio re-init failed after daemon restart: {exc}") + + after_sel = ad.current_selection() or {} + detected = ad.detect_plugged_profiles() or [] + after = { + "pactl_available": ad.pactl_available(), + "selection": after_sel, + "detected_profiles": [p.get("profile", {}).get("id") for p in detected], + } + return { + "ok": True, "best_effort": True, "flavour": flavour, + "kill": kill_info, "pya_reinitialized": pya_closed, + "apply_result": apply_result, + "input_recovered": bool(after_sel.get("source")), + "output_recovered": bool(after_sel.get("sink")), + "before": before, "after": after, + "hint": ("Soft reset only fixes Pulse-side state. If " + "input_recovered is False, try POST /api/audio/usb-reset " + "or physically replug the dongle."), + } + return await asyncio.to_thread(_do) + finally: + _RESET_LOCK.release() + + +def _find_usb_devices_by_vid_pid(vid: str, pid: str) -> list[str]: + """Return sysfs bus-id strings (e.g. '1-3') for every USB device whose + idVendor/idProduct match. Empty list when nothing matches. + + We read /sys/bus/usb/devices/* — every USB *device* (not interface) has + idVendor/idProduct files. Interfaces (paths with a colon, e.g. '1-3:1.1') + do not, so they're naturally skipped. + """ + import glob + hits: list[str] = [] + for path in glob.glob("/sys/bus/usb/devices/*"): + name = os.path.basename(path) + if ":" in name: + continue + try: + with open(os.path.join(path, "idVendor")) as f: + v = f.read().strip().lower() + with open(os.path.join(path, "idProduct")) as f: + p = f.read().strip().lower() + except OSError: + continue + if v == vid.lower() and p == pid.lower(): + hits.append(name) + return hits + + +def _snd_usb_interfaces_for_device(bus_id: str) -> list[str]: + """For USB device `bus_id` (e.g. '1-3'), return all interface names that + are currently bound to the snd-usb-audio driver (e.g. ['1-3:1.0']). + + Used so we unbind ONLY the audio interfaces and don't touch HID / HUB + interfaces on the same composite device. + """ + import glob + bound: list[str] = [] + base = f"/sys/bus/usb/devices/{bus_id}" + for iface in glob.glob(f"{base}/{bus_id}:*"): + driver_link = os.path.join(iface, "driver") + if not os.path.islink(driver_link): + continue + try: + driver = os.path.basename(os.readlink(driver_link)) + except OSError: + continue + if driver == "snd-usb-audio": + bound.append(os.path.basename(iface)) + return bound + + +def _write_sysfs(path: str, value: str) -> tuple[bool, str]: + """Write `value` to a sysfs file. Returns (success, error_message). + + Writes to /sys/bus/usb/drivers/snd-usb-audio/{bind,unbind} usually + require root. If permission denied, the caller should fall back to + invoking shell_scripts/reset_anker_usb.sh via sudo (one-time sudoers + setup documented in that script's header). + """ + try: + with open(path, "w") as f: + f.write(value) + return True, "" + except PermissionError as exc: + return False, f"permission denied: {path} ({exc})" + except OSError as exc: + return False, f"write failed: {path} ({exc})" + + +@router.post("/usb-reset") +async def usb_reset_anker(): + """HARD reset — unbind+rebind snd-usb-audio for the Anker (VID:PID + 291a:3301). Forces the kernel to re-parse the USB Audio Class + descriptors, which is the only way to recover a missing capture profile + on this Jetson without a physical replug. + + Tries two paths: + 1. Direct sysfs write (no sudo) — works if a udev rule has set + `audio` group ownership / world-write on the snd-usb-audio bind + files, or if Sanad runs as root (it shouldn't). + 2. Fallback to `sudo shell_scripts/reset_anker_usb.sh` — works after + a one-time sudoers entry; see that script's header for setup. + + Refuses while Live Gemini or a record playback is in flight (same + guard as the soft reset). + """ + if not _USB_RESET_LOCK.acquire(blocking=False): + raise HTTPException(429, "USB reset already in progress.") + try: + _refuse_if_busy() + + # Find candidate Anker USB devices currently enumerated. + candidates: list[dict] = [] + for tgt in _USB_RESET_TARGETS: + for bus_id in _find_usb_devices_by_vid_pid(tgt["vid"], tgt["pid"]): + candidates.append({"bus_id": bus_id, **tgt}) + if not candidates: + wanted = ", ".join( + "{}:{}".format(t["vid"], t["pid"]) for t in _USB_RESET_TARGETS + ) + raise HTTPException( + 404, + f"No matching USB device found (looked for {wanted}). " + "Plug the Anker dongle and try again.", + ) + + log.info("usb reset: candidates=%s", candidates) + + def _do() -> dict: + before_detected = [ + p.get("profile", {}).get("id") + for p in (ad.detect_plugged_profiles() or []) + ] + results: list[dict] = [] + for cand in candidates: + bus = cand["bus_id"] + ifaces = _snd_usb_interfaces_for_device(bus) + attempt = {"bus_id": bus, "label": cand["label"], + "snd_interfaces": ifaces, "method": None, + "ok": False, "error": ""} + if not ifaces: + attempt["error"] = ("no snd-usb-audio interfaces bound " + "to this device — already unbound or " + "kernel didn't claim it") + results.append(attempt) + continue + + # ─── Path 1: direct sysfs write ─── + unbind_path = "/sys/bus/usb/drivers/snd-usb-audio/unbind" + bind_path = "/sys/bus/usb/drivers/snd-usb-audio/bind" + direct_ok = True + direct_err = "" + for iface in ifaces: + ok, err = _write_sysfs(unbind_path, iface) + if not ok: + direct_ok = False + direct_err = err + break + if direct_ok: + import time as _time + _time.sleep(0.5) + for iface in ifaces: + ok, err = _write_sysfs(bind_path, iface) + if not ok: + direct_ok = False + direct_err = err + break + if direct_ok: + attempt.update({"method": "direct-sysfs", "ok": True}) + results.append(attempt) + continue + + # ─── Path 2: sudo helper script ─── + from pathlib import Path as _Path + helper = (_Path(__file__).resolve().parent.parent.parent + / "shell_scripts" / "reset_anker_usb.sh") + if not helper.exists(): + attempt.update({"method": "direct-sysfs", + "error": f"{direct_err}; helper not present " + f"at {helper}"}) + results.append(attempt) + continue + try: + r = subprocess.run( + ["sudo", "-n", str(helper), bus], + check=False, capture_output=True, text=True, timeout=10.0, + ) + attempt["method"] = "sudo-helper" + if r.returncode == 0: + attempt["ok"] = True + else: + attempt["error"] = ( + f"sudo helper exited {r.returncode}: " + f"{(r.stderr or r.stdout or '').strip()[:300]}" + ) + except subprocess.TimeoutExpired: + attempt["error"] = "sudo helper timed out (>10s)" + except FileNotFoundError as exc: + attempt["error"] = f"sudo not available: {exc}" + results.append(attempt) + + # Settle, then re-detect + import time as _time + _time.sleep(1.0) + try: + ad.apply_current_selection() + except Exception: + pass + try: + from Project.Sanad.main import audio_mgr + if audio_mgr is not None and hasattr(audio_mgr, "refresh_devices"): + audio_mgr.refresh_devices() + except Exception: + pass + + after_detected = [ + p.get("profile", {}).get("id") + for p in (ad.detect_plugged_profiles() or []) + ] + any_ok = any(r.get("ok") for r in results) + mic_now = any( + "anker" in (p.get("profile", {}).get("id") or "").lower() + for p in (ad.detect_plugged_profiles() or []) + ) + + return { + "ok": any_ok, + "candidates": results, + "before_detected_profiles": before_detected, + "after_detected_profiles": after_detected, + "input_recovered": mic_now, + "hint": ( + "If ok is False, the unbind/rebind path needs sudo. " + "Run `bash shell_scripts/reset_anker_usb.sh --setup-sudoers` " + "once on the robot to install the sudoers entry, then retry." + ) if not any_ok else None, + } + + return await asyncio.to_thread(_do) + finally: + _USB_RESET_LOCK.release() diff --git a/dashboard/routes/controller.py b/dashboard/routes/controller.py new file mode 100644 index 0000000..f0b998d --- /dev/null +++ b/dashboard/routes/controller.py @@ -0,0 +1,295 @@ +"""Controller tab — manual dashboard locomotion control (N2 Phase 1/2). + +Routes live under /api/controller. All WRITE actions (move / step / postures / +modes / MotionSwitcher) require the in-memory "Enable movement" arm flag and +return 409 when disarmed. Reads (/status, /joints, /msc, /status/summary), +E-STOP and the arm toggle are ALWAYS available. + +`/status/summary` is the aggregate the dashboard polls for the global subsystem +status strip (Camera / Face / Place / Movement). It is kept under /api/controller +(final path /api/controller/status/summary) so no second router is needed; note +/api/status (no /summary) is already used by the SPA, so the suffix matters. +""" + +from __future__ import annotations + +import asyncio + +from fastapi import APIRouter, HTTPException, Query +from pydantic import BaseModel + +from Project.Sanad.config import BASE_DIR +from Project.Sanad.core.logger import get_logger +from Project.Sanad.vision import recognition_state + +log = get_logger("controller_routes") + +router = APIRouter() + +STATE_PATH = BASE_DIR / "data" / ".recognition_state.json" + + +# ── lazy subsystem accessors ──────────────────────────────── + +def _get_loco(): + try: + from Project.Sanad.main import loco_controller # type: ignore + return loco_controller + except Exception: + return None + + +def _get_camera(): + try: + from Project.Sanad.main import camera # type: ignore + return camera + except Exception: + return None + + +def _get_live_sub(): + try: + from Project.Sanad.main import live_sub # type: ignore + return live_sub + except Exception: + return None + + +def _get_dispatch(): + try: + from Project.Sanad.main import movement_dispatch # type: ignore + return movement_dispatch + except Exception: + return None + + +def _require_loco(): + lc = _get_loco() + if lc is None: + raise HTTPException(503, "Locomotion controller subsystem unavailable.") + return lc + + +def _require_armed(lc): + if not lc.is_armed(): + raise HTTPException(409, "Movement is disarmed. Enable movement first.") + + +# ── reads ─────────────────────────────────────────────────── + +@router.get("/status") +async def get_status(): + lc = _require_loco() + return await asyncio.to_thread(lc.status) + + +@router.get("/joints") +async def get_joints(): + lc = _require_loco() + return await asyncio.to_thread(lc.joints) + + +@router.get("/msc") +async def get_msc(): + lc = _require_loco() + return await asyncio.to_thread(lc.msc_check) + + +# ── arm flag / E-STOP (always available) ──────────────────── + +@router.post("/arm") +async def set_arm(on: bool = Query(...)): + lc = _require_loco() + res = await asyncio.to_thread(lc.arm_movement if on else lc.disarm_movement) + return res + + +@router.post("/gemini-movement") +async def set_gemini_movement(on: bool = Query(...)): + """Enable / disable Gemini voice-driven locomotion (N2 Phase 3 gate). + + Writes recognition_state.movement_enabled — SEPARATE from the manual arm + flag. The Gemini child announces the toggle (spoken), and the parent + MovementDispatcher starts/stops acting on confirmation phrases. Default OFF. + """ + st = await asyncio.to_thread(recognition_state.mutate, STATE_PATH, + movement_enabled=bool(on)) + # Enabling Gemini movement also clears any E-STOP latch on the dispatcher. + if on: + md = _get_dispatch() + if md is not None: + try: + md.clear_estop() + except Exception: + log.exception("clear_estop failed") + log.info("gemini-movement %s", "ON" if on else "OFF") + return {"ok": True, "movement_enabled": st.movement_enabled} + + +@router.post("/estop") +async def estop(): + lc = _require_loco() + res = await asyncio.to_thread(lc.estop) + # Full stop: drop the manual arm flag AND latch the voice dispatcher off, so + # no source (teleop, step, or voice dispatch) can keep driving the robot. The + # dispatcher latch is used instead of flipping movement_enabled so the Gemini + # child does not deliver a spoken "movement disabled" line during an E-STOP. + try: + await asyncio.to_thread(lc.disarm_movement) + except Exception: + log.exception("estop disarm failed") + md = _get_dispatch() + if md is not None: + try: + md.emergency_stop() + except Exception: + log.exception("estop dispatcher latch failed") + return {"ok": True, **res} + + +@router.post("/stop") +async def stop(): + lc = _require_loco() + # Allowed even when disarmed — StopMove is always safe. + res = await asyncio.to_thread(lc.stop_move) + return res + + +# ── movement (armed) ──────────────────────────────────────── + +class MoveBody(BaseModel): + vx: float = 0.0 + vy: float = 0.0 + vyaw: float = 0.0 + run: bool = False + + +@router.post("/move") +async def move(body: MoveBody): + lc = _require_loco() + _require_armed(lc) + return await asyncio.to_thread(lc.move, body.vx, body.vy, body.vyaw, body.run) + + +@router.post("/step") +async def step(dir: str = Query(...)): + lc = _require_loco() + _require_armed(lc) + res = await asyncio.to_thread(lc.step, dir) + if not res.get("ok"): + raise HTTPException(400, res.get("reason", "step failed")) + return res + + +# ── modes / postures (armed) ──────────────────────────────── + +@router.post("/mode/prep") +async def mode_prep(): + lc = _require_loco() + _require_armed(lc) + return await asyncio.to_thread(lc.prep_mode) + + +@router.post("/mode/ready") +async def mode_ready(): + lc = _require_loco() + _require_armed(lc) + return await asyncio.to_thread(lc.ready_start_mode) + + +@router.post("/posture/{name}") +async def posture(name: str): + lc = _require_loco() + _require_armed(lc) + res = await asyncio.to_thread(lc.posture, name) + if not res.get("ok") and res.get("reason"): + raise HTTPException(400, res["reason"]) + return res + + +@router.post("/balance") +async def balance(mode: int = Query(...)): + lc = _require_loco() + _require_armed(lc) + return await asyncio.to_thread(lc.set_balance_mode, mode) + + +@router.post("/height") +async def height(h: float = Query(...)): + lc = _require_loco() + _require_armed(lc) + return await asyncio.to_thread(lc.set_stand_height, h) + + +# ── MotionSwitcher / reconnect (armed) ────────────────────── + +@router.post("/msc/select-ai") +async def msc_select_ai(): + lc = _require_loco() + _require_armed(lc) + return await asyncio.to_thread(lc.msc_select_ai) + + +@router.post("/msc/release") +async def msc_release(): + lc = _require_loco() + _require_armed(lc) + return await asyncio.to_thread(lc.msc_release) + + +@router.post("/reconnect") +async def reconnect(): + lc = _require_loco() + _require_armed(lc) + return await asyncio.to_thread(lc.reconnect) + + +# ── aggregate subsystem summary (always available) ────────── + +@router.get("/status/summary") +async def status_summary(): + """Live on/off state for the header status strip. Never raises.""" + try: + st = recognition_state.read(STATE_PATH) + except Exception: + st = recognition_state.RecognitionState() + + cam = _get_camera() + camera_running = False + try: + camera_running = bool(cam is not None and cam.is_running()) + except Exception: + camera_running = False + + lc = _get_loco() + movement_armed = False + try: + movement_armed = bool(lc is not None and lc.is_armed()) + except Exception: + movement_armed = False + + sub = _get_live_sub() + gemini_running = False + try: + runner = getattr(sub, "is_running", None) + gemini_running = bool(callable(runner) and runner()) + except Exception: + gemini_running = False + + # Effective Gemini-movement = the file flag AND not latched off by an E-STOP. + md = _get_dispatch() + estopped = False + try: + estopped = bool(md is not None and md.is_estopped()) + except Exception: + estopped = False + + return { + "vision_enabled": st.vision_enabled, + "camera_running": camera_running, + "face_rec_enabled": st.face_rec_enabled, + "zone_rec_enabled": st.zone_rec_enabled, + "movement_armed": movement_armed, + "gemini_movement_enabled": st.movement_enabled and not estopped, + "gemini_running": gemini_running, + } diff --git a/dashboard/routes/macros.py b/dashboard/routes/macros.py index c4fcaab..56c3042 100644 --- a/dashboard/routes/macros.py +++ b/dashboard/routes/macros.py @@ -15,6 +15,22 @@ log = get_logger("macros_route") router = APIRouter() +def _block_if_movement_armed(): + """409 when locomotion movement is armed — arm motion is mutually exclusive + with walking. The arm controller's motion-block is the safety net.""" + try: + from Project.Sanad.main import loco_controller # type: ignore + armed = loco_controller is not None and loco_controller.is_armed() + except HTTPException: + raise + except Exception: + return + if armed: + raise HTTPException( + 409, "Arm actions are disabled while movement is enabled. " + "Disable movement in the Controller tab first.") + + class MacroName(BaseModel): name: str @@ -63,6 +79,7 @@ async def stop_recording(): @router.post("/play") async def play_macro(payload: MacroName): from Project.Sanad.main import brain + _block_if_movement_armed() return await brain.play_macro(payload.name) @@ -157,6 +174,8 @@ async def play_combined(payload: ComboPlayPayload): has_motion = payload.action_id is not None or bool(payload.motion_file) if not has_audio and not has_motion: raise HTTPException(400, "pick at least one of audio_file / action_id / motion_file") + if has_motion: + _block_if_movement_armed() # audio-only combos still allowed while armed tasks = [] result: dict = { diff --git a/dashboard/routes/motion.py b/dashboard/routes/motion.py index 34ced42..58cea95 100644 --- a/dashboard/routes/motion.py +++ b/dashboard/routes/motion.py @@ -10,6 +10,23 @@ from pydantic import BaseModel router = APIRouter() +def _block_if_movement_armed(): + """409 if locomotion movement is armed — arm actions are mutually exclusive + with walking. The arm controller's own motion-block is the safety net; this + just gives the dashboard a clear message instead of a silent no-op.""" + try: + from Project.Sanad.main import loco_controller # type: ignore + armed = loco_controller is not None and loco_controller.is_armed() + except HTTPException: + raise + except Exception: + return + if armed: + raise HTTPException( + 409, "Arm actions are disabled while movement is enabled. " + "Disable movement in the Controller tab first.") + + @router.get("/status") async def motion_status(): from Project.Sanad.main import arm @@ -33,6 +50,7 @@ async def trigger_action(payload: TriggerPayload): from Project.Sanad.main import arm if arm is None: raise HTTPException(503, "Arm controller not attached.") + _block_if_movement_armed() speed = max(0.1, min(payload.speed, 5.0)) diff --git a/dashboard/routes/records.py b/dashboard/routes/records.py index a4d26c9..8846e6d 100644 --- a/dashboard/routes/records.py +++ b/dashboard/routes/records.py @@ -111,8 +111,42 @@ async def play_record(payload: RecordPlay): from Project.Sanad.main import audio_mgr import asyncio - await asyncio.to_thread(audio_mgr.play_wav, raw_path) - return {"ok": True, "record_name": payload.record_name, "file_kind": payload.file_kind, "path": str(raw_path)} + # Fire-and-forget — play_wav blocks for the clip duration on the G1 + # DDS path, and the dashboard's pause / resume / stop / status calls + # need to be served while it's running. Without this, /play wouldn't + # return until the clip finished and the UI couldn't interact with + # the in-flight playback. + asyncio.create_task(asyncio.to_thread( + audio_mgr.play_wav, raw_path, payload.record_name, + )) + return {"ok": True, "record_name": payload.record_name, + "file_kind": payload.file_kind, "path": str(raw_path)} + + +@router.post("/pause") +async def pause_playback(): + from Project.Sanad.main import audio_mgr + return audio_mgr.pause_playback() + + +@router.post("/resume") +async def resume_playback(): + from Project.Sanad.main import audio_mgr + return audio_mgr.resume_playback() + + +@router.post("/stop") +async def stop_playback(): + from Project.Sanad.main import audio_mgr + import asyncio + await asyncio.to_thread(audio_mgr.stop_playback) + return {"ok": True, "stopped": True} + + +@router.get("/playback-status") +async def playback_status(): + from Project.Sanad.main import audio_mgr + return audio_mgr.playback_status() class RecordRename(BaseModel): diff --git a/dashboard/routes/replay.py b/dashboard/routes/replay.py index 7d30c7f..698bcca 100644 --- a/dashboard/routes/replay.py +++ b/dashboard/routes/replay.py @@ -21,6 +21,22 @@ log = get_logger("replay_route") router = APIRouter() +def _block_if_movement_armed(): + """409 when locomotion movement is armed — arm motion (replay / teaching) is + mutually exclusive with walking.""" + try: + from Project.Sanad.main import loco_controller # type: ignore + armed = loco_controller is not None and loco_controller.is_armed() + except HTTPException: + raise + except Exception: + return + if armed: + raise HTTPException( + 409, "Arm actions are disabled while movement is enabled. " + "Disable movement in the Controller tab first.") + + # -- models -- class ReplayRequest(BaseModel): @@ -94,6 +110,7 @@ _BG_TASKS: set[asyncio.Task] = set() async def test_replay(payload: ReplayRequest): """Test-play a motion file at the given speed.""" from Project.Sanad.main import arm + _block_if_movement_armed() if arm.is_busy: raise HTTPException(409, "Arm is busy.") path = safe_path_under(MOTIONS_DIR, payload.name) @@ -142,6 +159,7 @@ async def start_teaching(payload: TeachRequest): from Project.Sanad.main import teacher if teacher is None: raise HTTPException(503, "Teaching module not available.") + _block_if_movement_armed() if teacher.is_recording: raise HTTPException(409, "Teaching session already active.") existing = MOTIONS_DIR / f"{payload.name}.jsonl" diff --git a/dashboard/routes/temp_monitor.py b/dashboard/routes/temp_monitor.py new file mode 100644 index 0000000..266e7b4 --- /dev/null +++ b/dashboard/routes/temp_monitor.py @@ -0,0 +1,67 @@ +"""REST endpoints backing the 3D motor-temperature dashboard (N1). + +Serves the motor name/mesh mapping + thresholds, and a one-shot temperature +snapshot (the front-end's initial fetch fallback). The live stream is over +/ws/motor-temps (dashboard/websockets/motor_temps.py). The 3D view itself is +the static page at /static/temp3d/index.html. +""" + +from __future__ import annotations + +import time + +from fastapi import APIRouter + +from Project.Sanad.dashboard.temp_motor_map import ( + MOTOR_NAMES, + MOTOR_TO_MESH, + TEMP_HOT_THRESHOLD, + TEMP_MAX, + TEMP_MIN, + TEMP_WARM_THRESHOLD, + build_payload, +) + +router = APIRouter() + + +def _get_arm(): + """Lazy import — avoids a circular import on dashboard load.""" + try: + from Project.Sanad.main import arm # type: ignore + return arm + except Exception: + return None + + +@router.get("/mapping") +async def motor_mapping(): + """Motor id → name / mesh map + the temperature gradient thresholds.""" + return { + "motor_names": MOTOR_NAMES, + "motor_to_mesh": MOTOR_TO_MESH, + "thresholds": { + "min": TEMP_MIN, + "max": TEMP_MAX, + "warm": TEMP_WARM_THRESHOLD, + "hot": TEMP_HOT_THRESHOLD, + }, + } + + +@router.get("/motors") +async def motors_snapshot(): + """One-shot motor temperature + position snapshot (Marcus payload shape).""" + arm = _get_arm() + temps: list = [] + positions: list = [] + if arm is not None: + try: + temps = arm.get_motor_temps() + except Exception: + temps = [] + try: + positions = arm.get_current_q() + except Exception: + positions = [] + return build_payload(temps, positions, time.time()) diff --git a/dashboard/routes/zones.py b/dashboard/routes/zones.py new file mode 100644 index 0000000..395077b --- /dev/null +++ b/dashboard/routes/zones.py @@ -0,0 +1,421 @@ +"""Zones tab — zone → place → linked-faces management + "go here" destination. + +Hierarchy (replaces the old flat places): + Zone (name + description) + └─ Place (name + description + optional reference photos + linked face ids) + +Routes live under /api/zones. Toggle + CRUD changes write +data/.recognition_state.json (the SAME file faces use); the Gemini child polls +it at 1 Hz and re-primes / announces mid-session. The "go here" endpoints set a +navigation target the robot will head to once N2 locomotion is wired — for now +they just record the target and feed Gemini the place's reference. +""" + +from __future__ import annotations + +import io +from typing import Optional + +from fastapi import APIRouter, File, HTTPException, Query, UploadFile +from fastapi.responses import FileResponse, StreamingResponse +from pydantic import BaseModel + +from Project.Sanad.config import BASE_DIR +from Project.Sanad.core.logger import get_logger +from Project.Sanad.dashboard.routes._safe_io import check_upload_size +from Project.Sanad.vision import recognition_state + +log = get_logger("zones_routes") + +router = APIRouter() + +STATE_PATH = BASE_DIR / "data" / ".recognition_state.json" + + +# ── lazy subsystem accessors ──────────────────────────────── + +def _get_camera(): + try: + from Project.Sanad.main import camera # type: ignore + return camera + except Exception: + return None + + +def _get_zone_gallery(): + try: + from Project.Sanad.main import zone_gallery # type: ignore + return zone_gallery + except Exception: + return None + + +def _get_face_gallery(): + try: + from Project.Sanad.main import gallery # type: ignore + return gallery + except Exception: + return None + + +def _require_zones(): + g = _get_zone_gallery() + if g is None: + raise HTTPException(503, "Zone gallery subsystem unavailable.") + return g + + +def _bump_zones_version() -> int: + cur = recognition_state.read(STATE_PATH) + v = cur.zones_version + 1 + recognition_state.mutate(STATE_PATH, zones_version=v) + return v + + +def _validate_image(content: bytes, filename: str | None = None) -> None: + check_upload_size(content) + if len(content) < 16: + raise HTTPException(400, "Image too small / empty.") + if not (content[:3] == b"\xff\xd8\xff" or content[:8] == b"\x89PNG\r\n\x1a\n"): + raise HTTPException(400, f"Only JPEG/PNG accepted (got {filename or 'unknown'}).") + + +def _safe_photo_name(name: str) -> None: + if "/" in name or ".." in name or "\x00" in name: + raise HTTPException(400, "Invalid photo name.") + + +def _resolve_faces(face_ids: list[int]) -> list[dict]: + """Turn linked face ids into [{id, name}] using the face gallery.""" + g = _get_face_gallery() + out = [] + for fid in face_ids: + name = None + if g is not None: + try: + e = g.get(fid) + name = e.name if e else None + except Exception: + name = None + out.append({"id": fid, "name": name}) + return out + + +def _place_to_dict(p) -> dict: + d = p.to_dict() + d["faces"] = _resolve_faces(p.face_ids) + return d + + +def _zone_to_dict(z) -> dict: + return { + "id": z.id, "name": z.name, "description": z.description, + "added_at": z.added_at, + "places": [_place_to_dict(p) for p in z.places], + } + + +def _nav_target_dict(st, gallery) -> Optional[dict]: + zid, pid = st.nav_target_zone_id, st.nav_target_place_id + if not zid or not pid: + return None + zone_name = place_name = None + if gallery is not None: + try: + z = gallery.get_zone(zid) + zone_name = z.name if z else None + p = gallery.get_place(zid, pid) + place_name = p.name if p else None + except Exception: + pass + return {"zone_id": zid, "place_id": pid, + "zone_name": zone_name, "place_name": place_name} + + +# ── state + toggle ────────────────────────────────────────── + +@router.get("/state") +async def get_state(): + st = recognition_state.read(STATE_PATH) + g = _get_zone_gallery() + zones_count = places_count = 0 + if g is not None: + try: + zones = g.list_zones() + zones_count = len(zones) + places_count = sum(len(z.places) for z in zones) + except Exception: + pass + return { + "zone_rec_enabled": st.zone_rec_enabled, + "zones_version": st.zones_version, + "zones_count": zones_count, + "places_count": places_count, + "nav_target": _nav_target_dict(st, g), + } + + +@router.post("/zone-rec") +async def set_zone_rec(on: bool = Query(...)): + """Enable / disable the robot's knowledge of zones & places (hot).""" + st = recognition_state.mutate(STATE_PATH, zone_rec_enabled=bool(on)) + log.info("zone recognition %s", "ON" if on else "OFF") + return {"ok": True, "zone_rec_enabled": st.zone_rec_enabled} + + +@router.post("/sync") +async def sync_zones(): + v = _bump_zones_version() + log.info("zones sync requested → v.%d", v) + return {"ok": True, "zones_version": v} + + +# ── zones CRUD ────────────────────────────────────────────── + +class NamePayload(BaseModel): + name: Optional[str] = None + + +class DescribePayload(BaseModel): + description: Optional[str] = None + + +class FacesPayload(BaseModel): + face_ids: list[int] = [] + + +@router.get("") +async def list_zones(): + g = _require_zones() + zones = g.list_zones() + return {"zones": [_zone_to_dict(z) for z in zones], "total": len(zones)} + + +@router.post("/create") +async def create_zone(name: Optional[str] = Query(default=None), + description: Optional[str] = Query(default=None)): + g = _require_zones() + if not (name or "").strip() and not (description or "").strip(): + raise HTTPException(400, "A zone needs at least a name or a description.") + z = g.create_zone(name=name, description=description) + _bump_zones_version() + return {"ok": True, "zone": _zone_to_dict(z)} + + +@router.post("/{zone_id}/rename") +async def rename_zone(zone_id: int, payload: NamePayload): + g = _require_zones() + try: + g.rename_zone(zone_id, payload.name) + except FileNotFoundError as exc: + raise HTTPException(404, str(exc)) + _bump_zones_version() + return {"ok": True, "zone": _zone_to_dict(g.get_zone(zone_id))} + + +@router.post("/{zone_id}/describe") +async def describe_zone(zone_id: int, payload: DescribePayload): + g = _require_zones() + try: + g.describe_zone(zone_id, payload.description) + except FileNotFoundError as exc: + raise HTTPException(404, str(exc)) + _bump_zones_version() + return {"ok": True, "zone": _zone_to_dict(g.get_zone(zone_id))} + + +@router.delete("/{zone_id}") +async def delete_zone(zone_id: int): + g = _require_zones() + try: + g.delete_zone(zone_id) + except FileNotFoundError as exc: + raise HTTPException(404, str(exc)) + # If the active destination was inside this zone, clear it. + st = recognition_state.read(STATE_PATH) + if st.nav_target_zone_id == zone_id: + recognition_state.mutate(STATE_PATH, nav_target_zone_id=0, nav_target_place_id=0) + _bump_zones_version() + return {"ok": True, "deleted": zone_id} + + +# ── places CRUD (within a zone) ───────────────────────────── + +@router.post("/{zone_id}/places/create") +async def create_place( + zone_id: int, + name: Optional[str] = Query(default=None), + description: Optional[str] = Query(default=None), + face_ids: list[int] = Query(default=[]), + files: Optional[list[UploadFile]] = File(default=None), +): + g = _require_zones() + if g.get_zone(zone_id) is None: + raise HTTPException(404, f"zone_{zone_id} not found") + if not (name or "").strip() and not (description or "").strip(): + raise HTTPException(400, "A place needs at least a name or a description.") + image_bytes: list[bytes] = [] + for f in (files or []): + content = await f.read() + if not content: + continue + _validate_image(content, f.filename) + image_bytes.append(content) + p = g.create_place(zone_id, name=name, description=description, + face_ids=face_ids, image_bytes_list=image_bytes or None) + _bump_zones_version() + return {"ok": True, "place": _place_to_dict(p)} + + +@router.post("/{zone_id}/places/{place_id}/rename") +async def rename_place(zone_id: int, place_id: int, payload: NamePayload): + g = _require_zones() + try: + g.rename_place(zone_id, place_id, payload.name) + except FileNotFoundError as exc: + raise HTTPException(404, str(exc)) + _bump_zones_version() + return {"ok": True, "place": _place_to_dict(g.get_place(zone_id, place_id))} + + +@router.post("/{zone_id}/places/{place_id}/describe") +async def describe_place(zone_id: int, place_id: int, payload: DescribePayload): + g = _require_zones() + try: + g.describe_place(zone_id, place_id, payload.description) + except FileNotFoundError as exc: + raise HTTPException(404, str(exc)) + _bump_zones_version() + return {"ok": True, "place": _place_to_dict(g.get_place(zone_id, place_id))} + + +@router.post("/{zone_id}/places/{place_id}/faces") +async def set_place_faces(zone_id: int, place_id: int, payload: FacesPayload): + """Replace the set of saved faces linked to this place.""" + g = _require_zones() + try: + g.set_place_faces(zone_id, place_id, payload.face_ids) + except FileNotFoundError as exc: + raise HTTPException(404, str(exc)) + _bump_zones_version() + return {"ok": True, "place": _place_to_dict(g.get_place(zone_id, place_id))} + + +@router.post("/{zone_id}/places/{place_id}/capture") +async def capture_to_place(zone_id: int, place_id: int): + g = _require_zones() + cam = _get_camera() + if cam is None or not cam.is_running(): + raise HTTPException(409, "Camera is not running. Toggle Vision ON first.") + jpeg = cam.get_fresh_frame(max_age_s=0.5, timeout_s=1.5) + if not jpeg: + raise HTTPException(409, "Camera has no frame yet.") + try: + fname = g.add_photo(zone_id, place_id, jpeg) + except FileNotFoundError as exc: + raise HTTPException(404, str(exc)) + _bump_zones_version() + return {"ok": True, "added": fname, "place": _place_to_dict(g.get_place(zone_id, place_id))} + + +@router.post("/{zone_id}/places/{place_id}/upload") +async def upload_to_place(zone_id: int, place_id: int, + files: list[UploadFile] = File(...)): + g = _require_zones() + if g.get_place(zone_id, place_id) is None: + raise HTTPException(404, f"zone_{zone_id}/place_{place_id} not found") + added: list[str] = [] + for f in files: + content = await f.read() + _validate_image(content, f.filename) + try: + added.append(g.add_photo(zone_id, place_id, content)) + except FileNotFoundError as exc: + raise HTTPException(404, str(exc)) + _bump_zones_version() + return {"ok": True, "added": added, "place": _place_to_dict(g.get_place(zone_id, place_id))} + + +@router.delete("/{zone_id}/places/{place_id}") +async def delete_place(zone_id: int, place_id: int): + g = _require_zones() + try: + g.delete_place(zone_id, place_id) + except FileNotFoundError as exc: + raise HTTPException(404, str(exc)) + st = recognition_state.read(STATE_PATH) + if st.nav_target_zone_id == zone_id and st.nav_target_place_id == place_id: + recognition_state.mutate(STATE_PATH, nav_target_zone_id=0, nav_target_place_id=0) + _bump_zones_version() + return {"ok": True, "deleted": place_id} + + +@router.delete("/{zone_id}/places/{place_id}/photo/{photo_name}") +async def delete_place_photo(zone_id: int, place_id: int, photo_name: str): + g = _require_zones() + _safe_photo_name(photo_name) + try: + g.delete_photo(zone_id, place_id, photo_name) + except FileNotFoundError as exc: + raise HTTPException(404, str(exc)) + _bump_zones_version() + return {"ok": True, "deleted": photo_name} + + +@router.get("/{zone_id}/places/{place_id}/photo/{photo_name}") +async def get_place_photo(zone_id: int, place_id: int, photo_name: str, + download: int = Query(default=0)): + g = _require_zones() + _safe_photo_name(photo_name) + path = g.get_photo(zone_id, place_id, photo_name) + if path is None: + raise HTTPException(404, "Photo not found.") + media = "image/png" if path.suffix.lower() == ".png" else "image/jpeg" + headers = {} + if download: + headers["Content-Disposition"] = ( + f'attachment; filename="zone_{zone_id}_place_{place_id}_{photo_name}"') + return FileResponse(path, media_type=media, headers=headers) + + +@router.get("/{zone_id}/places/{place_id}/download.zip") +async def download_place_zip(zone_id: int, place_id: int): + g = _require_zones() + try: + data = g.zip_place(zone_id, place_id) + except FileNotFoundError as exc: + raise HTTPException(404, str(exc)) + return StreamingResponse( + io.BytesIO(data), media_type="application/zip", + headers={ + "Content-Disposition": f'attachment; filename="zone_{zone_id}_place_{place_id}.zip"', + "Content-Length": str(len(data)), + }, + ) + + +# ── "go here" navigation target ───────────────────────────── + +@router.post("/{zone_id}/places/{place_id}/go") +async def go_to_place(zone_id: int, place_id: int): + """Set this place as the active destination. Records the target and lets + the Gemini child pick it up (reference photo + goal). Actual robot motion + is wired by N2 locomotion — until then this just establishes the goal.""" + g = _require_zones() + p = g.get_place(zone_id, place_id) + if p is None: + raise HTTPException(404, f"zone_{zone_id}/place_{place_id} not found") + recognition_state.mutate(STATE_PATH, + nav_target_zone_id=zone_id, + nav_target_place_id=place_id) + log.info("nav target set → zone_%d/place_%d (%s)", zone_id, place_id, + p.name or "(unnamed)") + return {"ok": True, "nav_target": {"zone_id": zone_id, "place_id": place_id, + "place_name": p.name}} + + +@router.post("/nav/clear") +async def clear_nav_target(): + recognition_state.mutate(STATE_PATH, nav_target_zone_id=0, nav_target_place_id=0) + log.info("nav target cleared") + return {"ok": True, "nav_target": None} diff --git a/dashboard/static/index.html b/dashboard/static/index.html index 086b375..754fac6 100644 --- a/dashboard/static/index.html +++ b/dashboard/static/index.html @@ -5,7 +5,7 @@