546 lines
26 KiB
Python

#!/usr/bin/env python3
"""Sanad — unified robot assistant entry point.
Starts all subsystems and the FastAPI dashboard.
python main.py # default port 8000
python main.py --port 8080 # custom port
"""
from __future__ import annotations
import argparse
import importlib
import os
import sys
import types
from pathlib import Path
# ─────────────────────────────────────────────────────────────────────────────
# Layout detection — support BOTH:
# 1. Dev layout: <root>/Project/Sanad/main.py (imports use Project.Sanad.*)
# 2. Deployed layout: /home/unitree/Sanad/main.py (no Project/ wrapper)
#
# In the deployed case we synthesize a `Project` namespace package and alias
# `Project.Sanad` → the local `Sanad` package, so every `from Project.Sanad.X
# import Y` keeps working without rewriting any other file.
# ─────────────────────────────────────────────────────────────────────────────
_THIS_DIR = Path(__file__).resolve().parent # .../Sanad
_PARENT = _THIS_DIR.parent # .../Project OR /home/unitree
if _PARENT.name == "Project":
# Dev layout — add the directory containing Project/
_ROOT = _PARENT.parent
if str(_ROOT) not in sys.path:
sys.path.insert(0, str(_ROOT))
else:
# Deployed layout — create a virtual Project package and alias
if str(_PARENT) not in sys.path:
sys.path.insert(0, str(_PARENT))
if "Project" not in sys.modules:
_proj = types.ModuleType("Project")
_proj.__path__ = [] # mark as namespace package
sys.modules["Project"] = _proj
if "Project.Sanad" not in sys.modules:
# Import the local Sanad package as a top-level module first
_sanad = importlib.import_module(_THIS_DIR.name)
sys.modules["Project.Sanad"] = _sanad
sys.modules["Project"].Sanad = _sanad # type: ignore[attr-defined]
# When main.py runs as a script (`python3 main.py`), Python loads it as the
# `__main__` module — NOT as `Project.Sanad.main`. Route handlers later do
# `from Project.Sanad.main import arm` etc; without the alias below, Python
# would re-execute this file from scratch under a different module name,
# creating a SECOND set of subsystem instances (uninitialised). Every
# `subsystem not available` / `No LowState` symptom traces back to this.
# The alias ensures both names point at the exact same module object.
if __name__ == "__main__":
sys.modules["Project.Sanad.main"] = sys.modules["__main__"]
# asyncio compat shim — backfills asyncio.to_thread for Python 3.8.
# MUST be imported before any other Sanad module that uses asyncio.to_thread.
from Project.Sanad.core import asyncio_compat # noqa: F401
from Project.Sanad.config import (
DASHBOARD_HOST,
DASHBOARD_PORT,
DASHBOARD_INTERFACE,
DDS_NETWORK_INTERFACE,
)
from Project.Sanad.core.logger import get_logger
log = get_logger("main")
def _safe_import(label: str, importer):
"""Import a module by callable, returning None if it fails."""
try:
return importer()
except Exception:
log.exception("Failed to import %s — that subsystem will be unavailable", label)
return None
def _safe_construct(name: str, factory):
"""Construct a subsystem, log + return None on failure."""
if factory is None:
return None
try:
return factory()
except Exception:
log.exception("Failed to construct %s — that subsystem will be unavailable", name)
return None
# ── isolated imports — one bad module never blocks the others ──
Brain = _safe_import("Brain", lambda: __import__("Project.Sanad.core.brain", fromlist=["Brain"]).Brain)
ArmController = _safe_import("ArmController", lambda: __import__("Project.Sanad.motion.arm_controller", fromlist=["ArmController"]).ArmController)
MacroPlayer = _safe_import("MacroPlayer", lambda: __import__("Project.Sanad.motion.macro_player", fromlist=["MacroPlayer"]).MacroPlayer)
MacroRecorder = _safe_import("MacroRecorder", lambda: __import__("Project.Sanad.motion.macro_recorder", fromlist=["MacroRecorder"]).MacroRecorder)
TeachingSession = _safe_import("TeachingSession", lambda: __import__("Project.Sanad.motion.teaching", fromlist=["TeachingSession"]).TeachingSession)
AudioManager = _safe_import("AudioManager", lambda: __import__("Project.Sanad.voice.audio_manager", fromlist=["AudioManager"]).AudioManager)
LocalTTSEngine = _safe_import("LocalTTSEngine", lambda: __import__("Project.Sanad.voice.local_tts", fromlist=["LocalTTSEngine"]).LocalTTSEngine)
WakePhraseManager = _safe_import("WakePhraseManager", lambda: __import__("Project.Sanad.voice.wake_phrase_manager", fromlist=["WakePhraseManager"]).WakePhraseManager)
LiveVoiceLoop = _safe_import("LiveVoiceLoop", lambda: __import__("Project.Sanad.voice.live_voice_loop", fromlist=["LiveVoiceLoop"]).LiveVoiceLoop)
TypedReplayEngine = _safe_import("TypedReplayEngine", lambda: __import__("Project.Sanad.voice.typed_replay", fromlist=["TypedReplayEngine"]).TypedReplayEngine)
GeminiVoiceClient = _safe_import("GeminiVoiceClient", lambda: __import__("Project.Sanad.gemini.client", fromlist=["GeminiVoiceClient"]).GeminiVoiceClient)
GeminiSubprocess = _safe_import("GeminiSubprocess", lambda: __import__("Project.Sanad.gemini.subprocess", fromlist=["GeminiSubprocess"]).GeminiSubprocess)
LocalSubprocess = _safe_import("LocalSubprocess", lambda: __import__("Project.Sanad.local.subprocess", fromlist=["LocalSubprocess"]).LocalSubprocess)
CameraDaemon = _safe_import("CameraDaemon", lambda: __import__("Project.Sanad.vision.camera", fromlist=["CameraDaemon"]).CameraDaemon)
FaceGallery = _safe_import("FaceGallery", lambda: __import__("Project.Sanad.vision.face_gallery", fromlist=["FaceGallery"]).FaceGallery)
ZoneGallery = _safe_import("ZoneGallery", lambda: __import__("Project.Sanad.vision.zone_gallery", fromlist=["ZoneGallery"]).ZoneGallery)
LocoController = _safe_import("LocoController", lambda: __import__("Project.Sanad.G1_Controller.loco_controller", fromlist=["LocoController"]).LocoController)
MovementDispatcher = _safe_import("MovementDispatcher", lambda: __import__("Project.Sanad.voice.movement_dispatch", fromlist=["MovementDispatcher"]).MovementDispatcher)
# ── global instances (imported by route modules) ──
brain = _safe_construct("brain", Brain) if Brain else None
arm = _safe_construct("arm", ArmController)
audio_mgr = _safe_construct("audio_mgr", AudioManager)
voice_client = _safe_construct("voice_client", GeminiVoiceClient)
local_tts = _safe_construct("local_tts", LocalTTSEngine)
wake_mgr = _safe_construct("wake_mgr", WakePhraseManager)
macro_rec = _safe_construct("macro_rec", (lambda: MacroRecorder(arm)) if (MacroRecorder and arm) else None)
macro_play = _safe_construct("macro_play", (lambda: MacroPlayer(audio_mgr, arm)) if (MacroPlayer and arm) else None)
teacher = _safe_construct("teacher", (lambda: TeachingSession(arm)) if (TeachingSession and arm) else None)
live_voice = _safe_construct("live_voice", (lambda: LiveVoiceLoop(voice_client, arm, wake_mgr, audio_mgr)) if (LiveVoiceLoop and voice_client and arm and wake_mgr and audio_mgr) else None)
# Which voice supervisor to mount. SANAD_VOICE_BRAIN chooses the brain
# that runs INSIDE the subprocess (see voice/sanad_voice.py); the same
# env var picks WHICH supervisor here manages that subprocess so its
# log-line parser matches the brain's emit format.
_brain_choice = os.environ.get("SANAD_VOICE_BRAIN", "gemini").strip().lower()
if _brain_choice == "local" and LocalSubprocess is not None:
live_sub = _safe_construct("live_sub", LocalSubprocess)
else:
live_sub = _safe_construct("live_sub", GeminiSubprocess)
typed_replay = _safe_construct("typed_replay", (lambda: TypedReplayEngine(voice_client, audio_mgr)) if (TypedReplayEngine and voice_client and audio_mgr) else None)
# ── Locomotion controller (N2) — manual dashboard locomotion ────────────────
# Reuses the arm controller's single ChannelFactoryInitialize (one DDS init per
# process) — it does NOT init DDS itself. Disarmed every boot. See
# G1_Controller/loco_controller.py and dashboard/routes/controller.py.
loco_controller = _safe_construct(
"loco_controller",
(lambda: LocoController(arm)) if (LocoController and arm) else None)
# Arm ⇄ locomotion mutual exclusion: the arm must NEVER run a replay / SDK
# action / gesture while the robot may be walking. `movement_active` is True for
# the MANUAL gate (armed/teleop) AND for ~1.5s after any move/step — so it also
# covers Phase-3 Gemini-driven moves (which call loco.move/step directly).
# Checked at every arm playback chokepoint (replay_file / _execute), so it blocks
# voice/Gemini-triggered gestures too, not just the dashboard.
if arm is not None and loco_controller is not None:
try:
if hasattr(arm, "set_motion_block"):
arm.set_motion_block(loco_controller.movement_active)
log.info("Arm motion-block wired to locomotion movement_active")
except Exception:
log.exception("Could not wire arm motion-block")
# ── Gemini voice → movement dispatcher (N2 Phase 3) ─────────────────────────
# Reads Gemini's spoken (BOT) transcript via the live supervisor's bot-callback
# and drives loco_controller on a confirmation-phrase match (Marcus pattern).
# Gated on recognition_state.movement_enabled (the "Enable Gemini movement"
# toggle) — SEPARATE from the manual arm flag. Inert until that flag is on.
movement_dispatch = None
if MovementDispatcher and loco_controller is not None:
try:
from Project.Sanad.config import BASE_DIR as _BD2, MOTIONS_DIR as _MD
movement_dispatch = _safe_construct(
"movement_dispatch",
lambda: MovementDispatcher(
loco_controller,
_MD / "instruction.json",
_BD2 / "data" / ".recognition_state.json"))
if movement_dispatch is not None:
movement_dispatch.start()
if live_sub is not None and hasattr(live_sub, "register_bot_callback"):
live_sub.register_bot_callback(movement_dispatch.on_bot_text)
log.info("Movement dispatcher wired to Gemini BOT transcript")
except Exception:
log.exception("Could not wire movement dispatcher")
# ── Recognition (camera + face gallery) ─────────────────────────────────────
# Camera is idle until the dashboard toggles vision on; face gallery is pure
# file IO and always available if the import succeeded.
#
# Config precedence (highest first): explicit env var → config/core_config.json
# section → hardcoded default. The parent process normally has no SANAD_CAMERA_*
# env vars (LIVE_TUNE is only forwarded to the Gemini child), so in practice the
# core_config.json `camera` / `faces` sections are the live source here.
def _build_camera():
from Project.Sanad.core.config_loader import section as _cfg_section
cam_cfg = _cfg_section("core", "camera")
def _knob(env_key: str, cfg_key: str, default):
env_val = os.environ.get(env_key)
if env_val is not None and env_val != "":
return type(default)(env_val)
return type(default)(cam_cfg.get(cfg_key, default))
# Frames are cached in memory and pushed to the Gemini child over its
# stdin (see GeminiSubprocess._frame_forwarder) — no file drop.
return CameraDaemon(
width=_knob("SANAD_CAMERA_WIDTH", "width", 424),
height=_knob("SANAD_CAMERA_HEIGHT", "height", 240),
fps=_knob("SANAD_CAMERA_FPS", "fps", 15),
jpeg_quality=_knob("SANAD_CAMERA_JPEG_QUALITY", "jpeg_quality", 70),
stale_threshold_s=float(cam_cfg.get("stale_threshold_s", 10.0)),
reconnect_min_s=float(cam_cfg.get("reconnect_min_s", 2.0)),
reconnect_max_s=float(cam_cfg.get("reconnect_max_s", 10.0)),
capture_timeout_ms=int(cam_cfg.get("capture_timeout_ms", 5000)),
)
def _build_gallery():
from Project.Sanad.config import BASE_DIR
from Project.Sanad.core.config_loader import section as _cfg_section
faces_cfg = _cfg_section("core", "faces")
# SANAD_FACES_DIR is set absolute by LIVE_TUNE (the Gemini child reads the
# same var). In the parent it's usually unset → fall back to the JSON's
# dir_rel, then the hardcoded default. Honour absolute paths as-is.
raw = os.environ.get("SANAD_FACES_DIR") or faces_cfg.get("dir_rel", "data/faces")
p = Path(raw)
root = p if p.is_absolute() else (BASE_DIR / raw)
return FaceGallery(root)
def _build_zone_gallery():
# N3 — zones gallery (zone → place → linked faces). Honours SANAD_ZONES_DIR
# (absolute) then the core_config 'zones' section dir_rel, then a default.
from Project.Sanad.config import BASE_DIR
from Project.Sanad.core.config_loader import section as _cfg_section
zones_cfg = _cfg_section("core", "zones")
raw = os.environ.get("SANAD_ZONES_DIR") or zones_cfg.get("dir_rel", "data/zones")
p = Path(raw)
root = p if p.is_absolute() else (BASE_DIR / raw)
return ZoneGallery(root)
camera = _safe_construct("camera", _build_camera if CameraDaemon else None)
gallery = _safe_construct("gallery", _build_gallery if FaceGallery else None)
zone_gallery = _safe_construct("zone_gallery", _build_zone_gallery if ZoneGallery else None)
# Restore persisted vision_enabled at boot — start camera if the user left
# it on across a reboot. Face-rec state is read by the Gemini child directly.
try:
from Project.Sanad.vision import recognition_state as _recog_state
from Project.Sanad.config import BASE_DIR as _BD
_state = _recog_state.read(_BD / "data" / ".recognition_state.json")
if _state.vision_enabled and camera is not None:
if camera.start():
log.info("Camera vision restored from state (backend=%s)", camera.backend)
else:
log.warning("Camera vision was ON but no backend available — leaving OFF")
_recog_state.mutate(_BD / "data" / ".recognition_state.json",
vision_enabled=False)
except Exception:
log.exception("Could not restore recognition state")
# Hand the camera to the Gemini supervisor so it can forward frames to the
# child over stdin while a live session runs.
if live_sub is not None and camera is not None:
try:
if hasattr(live_sub, "attach_camera"):
live_sub.attach_camera(camera)
log.info("Camera attached to live subprocess supervisor")
except Exception:
log.exception("attach_camera failed")
# Hand the AudioManager to the supervisor so the audio watcher can keep
# PulseAudio defaults aligned with the live profile on every Anker
# plug/unplug. Without this, typed-replay / record playback would stay on
# the boot device even after the live session swapped to Anker.
if live_sub is not None and audio_mgr is not None:
try:
if hasattr(live_sub, "attach_audio_manager"):
live_sub.attach_audio_manager(audio_mgr)
log.info("AudioManager attached to live subprocess supervisor")
except Exception:
log.exception("attach_audio_manager failed")
# ── Motion-state → Gemini channel ───────────────────────────────────────────
# The arm controller emits motion.action_started / _done / _error on the bus.
# Forward each to the Gemini child as a 'state:' line so the live session can
# answer "what are you doing?" honestly. Sync handlers, fired via emit_sync
# from the arm's worker thread — send_state just writes to a pipe (cheap).
if live_sub is not None and hasattr(live_sub, "send_state"):
try:
from Project.Sanad.core.event_bus import bus as _bus
def _on_motion_started(action: str = "", **_kw):
live_sub.send_state("start", action)
def _on_motion_done(action: str = "", elapsed_sec=None,
failed: bool = False, **_kw):
# action_error already covered the failure case with a reason;
# here just emit complete (skip if it failed to avoid a dup).
if not failed:
live_sub.send_state("complete", action, elapsed_sec=elapsed_sec)
def _on_motion_error(action: str = "", reason: str = "", **_kw):
live_sub.send_state("error", action, reason=reason)
_bus.on("motion.action_started", _on_motion_started)
_bus.on("motion.action_done", _on_motion_done)
_bus.on("motion.action_error", _on_motion_error)
log.info("Motion-state → Gemini channel wired")
except Exception:
log.exception("Could not wire motion-state → Gemini channel")
# Wire everything into the Brain (only what was constructed)
def _safe_attach(method_name: str, value):
if brain is None or value is None:
return
method = getattr(brain, method_name, None)
if method is None:
return
try:
method(value)
except Exception:
log.exception("brain.%s failed", method_name)
_safe_attach("attach_voice", voice_client)
_safe_attach("attach_audio_manager", audio_mgr)
_safe_attach("attach_arm", arm)
_safe_attach("attach_macro_recorder", macro_rec)
_safe_attach("attach_macro_player", macro_play)
_safe_attach("attach_live_voice", live_voice)
# ── Runtime sanity report ────────────────────────────────────────────────
SUBSYSTEMS = {
"brain": brain,
"arm": arm,
"audio_mgr": audio_mgr,
"voice_client": voice_client,
"local_tts": local_tts,
"macro_rec": macro_rec,
"macro_play": macro_play,
"teacher": teacher,
"wake_mgr": wake_mgr,
"live_voice": live_voice,
"live_sub": live_sub,
"typed_replay": typed_replay,
"camera": camera,
"gallery": gallery,
"zone_gallery": zone_gallery,
"loco_controller": loco_controller,
"movement_dispatch": movement_dispatch,
}
# Critical subsystems — if any of these are None, log a warning at startup
CRITICAL_SUBSYSTEMS = ("brain",)
for _name in CRITICAL_SUBSYSTEMS:
if SUBSYSTEMS.get(_name) is None:
log.error("CRITICAL subsystem '%s' is None — application will be unusable", _name)
_available = [k for k, v in SUBSYSTEMS.items() if v is not None]
_missing = [k for k, v in SUBSYSTEMS.items() if v is None]
log.info("Subsystems available (%d): %s", len(_available), ", ".join(_available))
if _missing:
log.warning("Subsystems unavailable (%d): %s", len(_missing), ", ".join(_missing))
_already_shut_down = False
def _do_shutdown(from_signal: bool = False):
"""Clean shutdown — release hardware, stop background tasks. Idempotent."""
global _already_shut_down
if _already_shut_down:
return
_already_shut_down = True
log.info("Shutdown requested")
if arm is not None:
try:
if hasattr(arm, "cancel"):
arm.cancel()
except Exception:
log.exception("arm.cancel() failed")
try:
if hasattr(arm, "disable"):
arm.disable()
except Exception:
log.exception("arm.disable() failed")
if movement_dispatch is not None:
try:
movement_dispatch.stop()
except Exception:
log.exception("movement_dispatch.stop() failed")
if loco_controller is not None:
try:
loco_controller.shutdown() # StopMove (no FSM change) + disarm
except Exception:
log.exception("loco_controller.shutdown() failed")
if live_sub is not None:
try:
running = live_sub.is_running() if callable(getattr(live_sub, "is_running", None)) else False
if running:
live_sub.stop()
except Exception:
log.exception("live_sub.stop() failed")
if audio_mgr is not None:
try:
if hasattr(audio_mgr, "close"):
audio_mgr.close()
except Exception:
log.exception("audio_mgr.close() failed")
if camera is not None:
try:
if camera.is_running():
camera.stop()
except Exception:
log.exception("camera.stop() failed")
log.info("Shutdown complete")
import atexit # noqa: E402
atexit.register(_do_shutdown)
# NOTE: Do NOT install custom SIGINT/SIGTERM handlers here.
# Uvicorn installs its own signal handlers for graceful shutdown.
# If we override them, Ctrl+C never reaches uvicorn and the server
# keeps running forever. Our _do_shutdown runs via atexit instead.
def _print_env_diagnostic():
"""Print everything you'd need to debug a deployment issue."""
print("=" * 60)
print("SANAD ENVIRONMENT DIAGNOSTIC")
print("=" * 60)
print(f"Python: {sys.version}")
print(f"Executable: {sys.executable}")
print(f"Platform: {sys.platform}")
print(f"BASE_DIR: {_THIS_DIR}")
print(f"Parent: {_PARENT}")
print(f"Layout: {'dev (Project/Sanad)' if _PARENT.name == 'Project' else 'deployed (top-level Sanad)'}")
print(f"Dashboard: {DASHBOARD_HOST}:{DASHBOARD_PORT} (interface: {DASHBOARD_INTERFACE})")
print(f"DDS interface: {DDS_NETWORK_INTERFACE}")
print()
print("sys.path[0:8]:")
for p in sys.path[:8]:
print(f" {p}")
print()
print("Critical imports:")
for mod_name in ("uvicorn", "fastapi", "pydantic", "starlette",
"websockets", "httpx", "pyaudio", "pyrealsense2",
"unitree_sdk2py", "ultralytics", "numpy", "cv2"):
try:
mod = __import__(mod_name)
ver = getattr(mod, "__version__", "?")
path = getattr(mod, "__file__", "?")
print(f"{mod_name:18s} {ver:12s} {path}")
except BaseException as exc:
print(f"{mod_name:18s} {type(exc).__name__}: {exc}")
print()
print("Subsystems available (after constructing main module globals):")
for name in sorted(SUBSYSTEMS):
print(f" {'' if SUBSYSTEMS[name] is not None else ''} {name}")
print("=" * 60)
def main():
parser = argparse.ArgumentParser(description="Sanad Robot Assistant")
parser.add_argument("--host", default=DASHBOARD_HOST,
help=f"Dashboard bind address. Default is wlan0's IP "
f"({DASHBOARD_HOST!r}). Override with SANAD_DASHBOARD_HOST "
f"or SANAD_DASHBOARD_INTERFACE.")
parser.add_argument("--port", type=int, default=DASHBOARD_PORT)
parser.add_argument("--network", default=DDS_NETWORK_INTERFACE,
help="DDS network interface (e.g. eth0, lo). "
"Override with SANAD_DDS_INTERFACE env var.")
parser.add_argument("--check-env", action="store_true",
help="Print environment diagnostic and exit "
"(no server start, no hardware init)")
args = parser.parse_args()
if args.check_env:
_print_env_diagnostic()
return
log.info("Sanad starting — Python %s @ %s", sys.version.split()[0], sys.executable)
log.info("BASE_DIR: %s", _THIS_DIR)
log.info("Dashboard interface: %s → bound to %s", DASHBOARD_INTERFACE, args.host)
log.info("Starting Sanad — host=%s port=%d network=%s", args.host, args.port, args.network)
if brain is not None:
try:
log.info("Brain status: %s", brain.status())
except Exception:
log.exception("brain.status() failed")
# Initialize hardware (graceful if unavailable)
if arm is not None:
try:
arm.init(network_interface=args.network)
except Exception:
log.exception("arm.init() failed — continuing without hardware")
# ── import uvicorn ──────────────────────────────────────────────────
# Catch ANY exception (not just ImportError) so the real failure reason
# is surfaced. The previous narrow catch hid issues like uvicorn pulling
# in a broken transitive dep, or being installed under a different
# site-packages than the active interpreter.
uvicorn = None
try:
import uvicorn # type: ignore
log.info("uvicorn %s loaded from %s",
getattr(uvicorn, "__version__", "?"),
getattr(uvicorn, "__file__", "?"))
except BaseException as exc:
log.error("Could not import uvicorn: %s: %s", type(exc).__name__, exc)
log.error("Python: %s", sys.executable)
log.error("sys.path[0:5]: %s", sys.path[:5])
log.error("Try: %s -m pip install --user 'uvicorn[standard]' fastapi", sys.executable)
sys.exit(1)
# ── import the FastAPI app ──────────────────────────────────────────
# Pass the app object directly so uvicorn doesn't have to re-resolve the
# import path (which differs between dev and deployed layouts).
try:
from Project.Sanad.dashboard.app import app as _app
except BaseException:
log.exception("Could not import Dashboard.app — aborting")
sys.exit(1)
# ── start the server ────────────────────────────────────────────────
try:
uvicorn.run(
_app,
host=args.host,
port=args.port,
log_level="info",
)
except BaseException:
log.exception("uvicorn.run() failed")
sys.exit(1)
if __name__ == "__main__":
main()