768 lines
34 KiB
Python
768 lines
34 KiB
Python
"""
|
|
marcus_brain.py — Marcus AI Brain Orchestrator
|
|
================================================
|
|
Shared brain logic for both terminal (run_marcus.py) and server (marcus_server.py).
|
|
|
|
Usage:
|
|
Terminal: python3 run_marcus.py
|
|
Server: python3 -m Server.marcus_server (imports init_brain + process_command)
|
|
"""
|
|
import json
|
|
import os
|
|
import re
|
|
import time
|
|
import sys
|
|
|
|
PROJECT_DIR = os.path.dirname(os.path.dirname(os.path.abspath(__file__)))
|
|
if PROJECT_DIR not in sys.path:
|
|
sys.path.insert(0, PROJECT_DIR)
|
|
|
|
from API.zmq_api import init_zmq, send_vel, gradual_stop, send_cmd
|
|
from API.camera_api import start_camera, stop_camera, get_frame
|
|
from API.yolo_api import (
|
|
init_yolo, yolo_summary, yolo_fps,
|
|
yolo_all_classes, yolo_closest, yolo_sees,
|
|
)
|
|
from API.odometry_api import init_odometry
|
|
from API.memory_api import init_memory, log_cmd, log_detection
|
|
from API.llava_api import (
|
|
OLLAMA_MODEL, ask, ask_talk, call_llava, parse_json, add_to_history,
|
|
)
|
|
from API.imgsearch_api import init_imgsearch, get_searcher
|
|
from Core.config_loader import load_config
|
|
from Core.logger import log as _log
|
|
|
|
from Brain.command_parser import try_local_command, init_autonomous
|
|
from Brain.executor import execute, execute_action
|
|
from Navigation.goal_nav import navigate_to_goal
|
|
from Navigation.patrol import patrol
|
|
from Autonomous.marcus_autonomous import AutonomousMode
|
|
|
|
_cfg = load_config("Brain")
|
|
|
|
_TALK_PATTERNS = [
|
|
# Questions
|
|
r"^(?:what|who|where|when|how|why|is|are|do|does|can|tell|describe|explain|show|analyze)\s+",
|
|
# Identity / facts told to the robot
|
|
r"^(?:my name is|i am|call me|that is|that person|note that|remember that)\s+",
|
|
# Acknowledgements
|
|
r"^(?:ok|okay|yes|no|good|nice|great|thanks|thank you|got it|understood|correct)\s*[!.]*$",
|
|
]
|
|
|
|
_NAT_GOAL_RE = re.compile(
|
|
r'^(?:keep\s+(?:turn|rotat|spin)\w*\s+\w+\s+until\s+(?:you\s+)?(?:see|find|spot)\s+.+'
|
|
r'|stop\s+when\s+(?:you\s+)?(?:see|find|spot)\s+.+'
|
|
r'|find\s+(?:a\s+|the\s+|me\s+a\s+)?\w.+'
|
|
r'|look\s+for\s+(?:a\s+|the\s+)?\w.+'
|
|
r'|search\s+for\s+(?:a\s+|the\s+)?\w.+)$',
|
|
re.IGNORECASE
|
|
)
|
|
|
|
|
|
# ══════════════════════════════════════════════════════════════════════════════
|
|
# INIT — called once by both run_marcus.py and marcus_server.py
|
|
# ══════════════════════════════════════════════════════════════════════════════
|
|
|
|
def init_brain():
|
|
"""Initialize all subsystems. Call once at startup from the parent process.
|
|
|
|
Optional subsystems (lidar / voice / imgsearch / autonomous) are gated on
|
|
`config_Brain.json::subsystems.<name>`. Disabling the ones you don't need
|
|
brings Marcus's boot time down from ~18 s to ~5-7 s.
|
|
"""
|
|
subsys = _cfg.get("subsystems", {}) or {}
|
|
|
|
# Bind the ZMQ PUB socket before anything tries to publish on it.
|
|
# This is now explicit (previously it happened as an import side effect,
|
|
# which crashed every multiprocessing child that re-imported zmq_api).
|
|
init_zmq()
|
|
|
|
raw_frame, raw_lock = start_camera()
|
|
# YOLO is optional on the Jetson: with Qwen2.5-VL loaded, YOLO's ~2 GiB
|
|
# of iGPU pushes Ollama into 30/70 CPU/GPU split and inference crawls.
|
|
# Set subsystems.yolo=false in config_Brain.json to skip it entirely;
|
|
# the VLM can describe the scene directly. Autonomous/patrol features
|
|
# that rely on YOLO degrade gracefully via the yolo_api stubs.
|
|
if subsys.get("yolo", True):
|
|
init_yolo(raw_frame, raw_lock)
|
|
else:
|
|
print(" [YOLO] disabled by config (subsystems.yolo=false) — saves ~2 GiB iGPU for VLM")
|
|
|
|
from API.zmq_api import get_socket
|
|
init_odometry(zmq_sock=get_socket())
|
|
|
|
init_memory()
|
|
|
|
# LiDAR — optional
|
|
if subsys.get("lidar", True):
|
|
try:
|
|
from API.lidar_api import init_lidar
|
|
init_lidar()
|
|
except Exception as e:
|
|
print(f" [LiDAR] Init failed: {e} — continuing without LiDAR")
|
|
else:
|
|
print(" [LiDAR] disabled by config")
|
|
|
|
# Image search — optional
|
|
if subsys.get("imgsearch", False):
|
|
init_imgsearch(
|
|
get_frame_fn=get_frame,
|
|
send_vel_fn=send_vel,
|
|
gradual_stop_fn=gradual_stop,
|
|
llava_fn=call_llava,
|
|
yolo_sees_fn=yolo_sees,
|
|
model=OLLAMA_MODEL,
|
|
)
|
|
else:
|
|
print(" [ImgSearch] disabled by config")
|
|
|
|
# Autonomous exploration mode — optional
|
|
if subsys.get("autonomous", True):
|
|
from API.memory_api import mem as _mem_ref
|
|
from API.llava_api import PATROL_PROMPT
|
|
auto = AutonomousMode(
|
|
get_frame_fn=get_frame,
|
|
send_vel_fn=send_vel,
|
|
gradual_stop_fn=gradual_stop,
|
|
yolo_sees_fn=yolo_sees,
|
|
yolo_summary_fn=yolo_summary,
|
|
yolo_all_classes_fn=yolo_all_classes,
|
|
yolo_closest_fn=yolo_closest,
|
|
odom_fn=lambda: {"x": 0, "y": 0, "heading": 0},
|
|
call_llava_fn=call_llava,
|
|
patrol_prompt=PATROL_PROMPT,
|
|
mem=_mem_ref,
|
|
)
|
|
from API.odometry_api import odom as _odom_ref, ODOM_AVAILABLE
|
|
if _odom_ref and ODOM_AVAILABLE:
|
|
auto._odom_pos = lambda: {
|
|
"x": _odom_ref._x, "y": _odom_ref._y, "heading": _odom_ref._heading
|
|
}
|
|
init_autonomous(auto)
|
|
else:
|
|
print(" [Autonomous] disabled by config")
|
|
|
|
send_cmd("start")
|
|
time.sleep(0.5)
|
|
send_cmd("walk")
|
|
time.sleep(0.5)
|
|
|
|
# Voice module — optional
|
|
if subsys.get("voice", True):
|
|
_init_voice()
|
|
else:
|
|
print(" [Voice] disabled by config")
|
|
|
|
_log("Brain initialized", "info", "brain")
|
|
|
|
# Synchronous warmup — same shape as Marcus_v1's marcus_llava.py. The
|
|
# Python process blocks here for ~60-90 s on the first run so the first
|
|
# real user command doesn't pay the cold-load. One attempt, no retry, no
|
|
# thread. By the time the dashboard prints, Qwen is resident in iGPU.
|
|
from API.llava_api import VLM_ENABLED, OLLAMA_HOST, _client as _llava_client
|
|
if not VLM_ENABLED:
|
|
print(" [VLM] disabled by config — safe mode (no Ollama load)")
|
|
else:
|
|
host_short = OLLAMA_HOST.replace("http://", "")
|
|
print(f" [VLM] target: {host_short} ({OLLAMA_MODEL})")
|
|
print(" [VLM] Warming up... (loading into iGPU — may take 60-90 s on cold start)")
|
|
try:
|
|
_llava_client.chat(
|
|
model=OLLAMA_MODEL,
|
|
messages=[{"role": "user", "content": "hi"}],
|
|
options={"temperature": 0.0, "num_predict": 5,
|
|
"num_batch": _cfg.get("num_batch", 16),
|
|
"num_ctx": _cfg.get("num_ctx", 1024)},
|
|
)
|
|
print(" [VLM] warm — first command will be fast")
|
|
except Exception as _e:
|
|
print(f" [VLM] warmup failed ({_e}) — first command may cold-load")
|
|
|
|
|
|
# Global voice references
|
|
_audio_api = None
|
|
_voice_module = None
|
|
|
|
|
|
def _init_voice():
|
|
"""
|
|
Initialize the voice subsystem: G1 built-in mic + Gemini Live S2S
|
|
(subprocess in gemini_sdk env, runs Voice/gemini_runner.py). Every
|
|
transcribed user command flows through process_command(); the brain
|
|
decides motion side-effects, but the verbal reply itself comes from
|
|
Gemini's voice through the G1 speaker (NOT TtsMaker — TtsMaker stays
|
|
for non-voice subsystem callers but isn't invoked from this callback).
|
|
"""
|
|
global _audio_api, _voice_module
|
|
try:
|
|
from API.audio_api import AudioAPI
|
|
from Voice.marcus_voice import VoiceModule
|
|
|
|
_audio_api = AudioAPI()
|
|
|
|
# Heuristic filter for unusable Gemini transcripts. Gemini emits
|
|
# `<noise>` literally when audio is non-speech and `.` for empty
|
|
# bursts. These shouldn't pollute the terminal or trigger motion.
|
|
# Bilingual aware: short Arabic motion commands like "اجلس" /
|
|
# "قف" / "تعال" are LEGIT — only reject short non-ASCII-non-Arabic
|
|
# snippets (Korean / Thai / etc — echo/distortion garbage).
|
|
def _is_supported_lang_char(c: str) -> bool:
|
|
# ASCII letters/digits/punct OR Arabic block (U+0600..U+06FF)
|
|
# OR whitespace.
|
|
return c.isascii() or "" <= c <= "ۿ" or c.isspace()
|
|
|
|
def _is_garbage_transcript(t: str) -> bool:
|
|
stripped = t.strip().strip(".!?,").strip()
|
|
if not stripped:
|
|
return True
|
|
low = stripped.lower()
|
|
if low in ("<noise>", "noise", "yeah", "ok", "okay", "uh", "um", "hmm", "mm"):
|
|
return True
|
|
# Bare single character (often "." → ".") or all punctuation.
|
|
if all(not c.isalnum() for c in stripped):
|
|
return True
|
|
# Reject only when text is BOTH short AND uses chars from
|
|
# neither English nor Arabic — those are echo/distortion
|
|
# mistranscriptions (Korean/Thai/etc fragments).
|
|
if len(stripped) <= 6:
|
|
if not all(_is_supported_lang_char(c) for c in stripped):
|
|
return True
|
|
return False
|
|
|
|
def _on_command(text, lang):
|
|
text = (text or "").strip()
|
|
if not text:
|
|
return
|
|
if _is_garbage_transcript(text):
|
|
# Skip silently — neither show nor dispatch to brain.
|
|
return
|
|
print(f' [Sanad] heard: "{text}"')
|
|
try:
|
|
result = process_command(text)
|
|
except Exception as e:
|
|
print(f" [Brain] Error processing voice command: {e}")
|
|
return
|
|
if isinstance(result, dict):
|
|
sp = (result.get("speak") or "").strip()
|
|
act = (result.get("action") or "").strip()
|
|
# In the Gemini S2S architecture Gemini owns the voice; we
|
|
# do NOT call audio_api.speak(sp) here (would collide with
|
|
# Gemini's own audio reply). Just show the operator what
|
|
# the brain decided so they can correlate motion with
|
|
# Gemini's spoken acknowledgement.
|
|
if act and act not in ("NONE", "TALK"):
|
|
print(f" [Sanad] doing: {act}{f' — {sp[:80]}' if sp else ''}")
|
|
elif sp:
|
|
print(f" [Brain] {sp[:120]}")
|
|
print("Command: ", end="", flush=True)
|
|
|
|
_voice_module = VoiceModule(_audio_api, on_command=_on_command)
|
|
_voice_module.start()
|
|
print(" [Voice] listening in background — say \"Sanad\" + your command")
|
|
except Exception as e:
|
|
print(f" [Voice] Init failed: {e} — continuing without voice")
|
|
_audio_api = None
|
|
_voice_module = None
|
|
|
|
|
|
# ══════════════════════════════════════════════════════════════════════════════
|
|
# PROCESS COMMAND — shared by terminal loop and WebSocket server
|
|
# ══════════════════════════════════════════════════════════════════════════════
|
|
|
|
def process_command(cmd: str) -> dict:
|
|
"""
|
|
Process a single command through the full brain pipeline.
|
|
Returns: {"type": str, "speak": str, "action": str, "elapsed": float}
|
|
|
|
Used by both run_marcus.py (terminal) and marcus_server.py (WebSocket).
|
|
"""
|
|
cmd = cmd.strip()
|
|
if not cmd:
|
|
return {"type": "empty", "speak": "", "action": "NONE", "elapsed": 0}
|
|
|
|
t0 = time.time()
|
|
|
|
# ── YOLO status ──────────────────────────────────────────────────────
|
|
if any(w in cmd.lower() for w in ("yolo", "what does yolo", "vision", "using yolo")):
|
|
from API.yolo_api import YOLO_AVAILABLE as _ya
|
|
status = "active" if _ya else "not loaded"
|
|
speak = f"YOLO: {status} | {yolo_summary()} | {yolo_fps():.1f}fps"
|
|
print(f" {speak}")
|
|
log_cmd(cmd, speak)
|
|
return {"type": "status", "speak": speak, "action": "YOLO", "elapsed": 0}
|
|
|
|
# ── Image search ─────────────────────────────────────────────────────
|
|
if cmd.lower().startswith("search/"):
|
|
speak = _handle_search(cmd)
|
|
return {"type": "search", "speak": speak, "action": "SEARCH", "elapsed": time.time() - t0}
|
|
|
|
# ── Auto-detect natural language goals ───────────────────────────────
|
|
if _NAT_GOAL_RE.match(cmd) and not cmd.lower().startswith("goal/"):
|
|
print(f" [Goal] Auto-detected: '{cmd}'")
|
|
navigate_to_goal(cmd.strip())
|
|
elapsed = time.time() - t0
|
|
log_cmd(cmd, f"Goal navigation: {cmd}", elapsed)
|
|
return {"type": "goal", "speak": f"Goal navigation: {cmd}", "action": "GOAL", "elapsed": elapsed}
|
|
|
|
# ── Explicit goal/ ───────────────────────────────────────────────────
|
|
if cmd.lower().startswith("goal/"):
|
|
goal = cmd[5:].strip()
|
|
if goal:
|
|
navigate_to_goal(goal)
|
|
elapsed = time.time() - t0
|
|
log_cmd(cmd, f"Goal navigation: {goal}", elapsed)
|
|
return {"type": "goal", "speak": f"Goal navigation: {goal}", "action": "GOAL", "elapsed": elapsed}
|
|
return {"type": "error", "speak": "Usage: goal/ stop when you see a person", "action": "NONE", "elapsed": 0}
|
|
|
|
# ── Autonomous patrol ────────────────────────────────────────────────
|
|
if cmd.lower().startswith("patrol"):
|
|
mins = 5.0
|
|
if " " in cmd:
|
|
try:
|
|
mins = float(cmd.split()[-1])
|
|
except ValueError:
|
|
pass
|
|
patrol(duration_minutes=mins)
|
|
elapsed = time.time() - t0
|
|
log_cmd(cmd, f"Patrol {mins}min", elapsed)
|
|
return {"type": "patrol", "speak": f"Patrol {mins}min complete", "action": "PATROL", "elapsed": elapsed}
|
|
|
|
# ── Local commands (place / odom / memory / help) ────────────────────
|
|
if try_local_command(cmd):
|
|
log_cmd(cmd, "local command")
|
|
return {"type": "local", "speak": "Done", "action": "LOCAL", "elapsed": time.time() - t0}
|
|
|
|
# ── Talk-only (questions / acknowledgements) ─────────────────────────
|
|
if any(re.match(p, cmd, re.IGNORECASE) for p in _TALK_PATTERNS):
|
|
speak = _handle_talk(cmd)
|
|
return {"type": "talk", "speak": speak, "action": "TALK", "elapsed": time.time() - t0}
|
|
|
|
# ── Greeting ─────────────────────────────────────────────────────────
|
|
if re.match(r"^(?:hi+|hey+|hello+|sup|yo+|greetings|good (?:morning|afternoon|evening))\s*[!.]*$",
|
|
cmd, re.IGNORECASE):
|
|
response = "Hello! I am Sanad. How can I help you?"
|
|
print(f"Sanad: {response}")
|
|
add_to_history(cmd, response)
|
|
log_cmd(cmd, response)
|
|
return {"type": "greeting", "speak": response, "action": "GREETING", "elapsed": 0}
|
|
|
|
# ── "Come to me" — smart YOLO-tracked approach ──────────────────────
|
|
# Old behaviour: walk forward 2s blindly. New: if YOLO is loaded,
|
|
# locate the person in the camera frame, turn toward them if off-
|
|
# centre, walk forward in short bursts, stop when 'arrived' (person
|
|
# fills enough of the frame to be 'right in front'). Bounded by
|
|
# COME_TO_ME_MAX_SEC; falls back to a 2s blind walk if YOLO isn't
|
|
# available so the canonical still does *something*.
|
|
#
|
|
# Arrival threshold: yolo_person_too_close(threshold=...) returns
|
|
# True if the person's bbox size_ratio (bbox_area / frame_area)
|
|
# exceeds the threshold. Defaults are calibrated for safety
|
|
# (0.25 = ~1m away, used by patrol). For come_here we want the
|
|
# robot to come MUCH closer — arm's length, ~0.5m — which is
|
|
# roughly size_ratio > 0.32 (lookup in marcus_yolo.distance_estimate:
|
|
# 'very close' starts at 0.30). 0.32 is a safe 'arrived' marker;
|
|
# any higher and the robot might bump into the user.
|
|
if re.match(r"^(?:come(?:\s+back)?(?:\s+to\s+me)?|come\s+here|get\s+closer|approach|move\s+closer)\s*[!.]*$", cmd, re.IGNORECASE):
|
|
from Brain.executor import move_step
|
|
try:
|
|
from API.yolo_api import yolo_sees, yolo_person_too_close, yolo_closest
|
|
from API.yolo_api import _stub_sees as _stub_sees_ref # type: ignore
|
|
yolo_active = (yolo_sees is not _stub_sees_ref)
|
|
except Exception:
|
|
yolo_active = False
|
|
|
|
COME_TO_ME_MAX_SEC = 25.0
|
|
COME_TO_ME_STEP_SEC = 0.6
|
|
COME_TO_ME_TURN_SEC = 0.3 # small correction (~14° at default vyaw)
|
|
COME_TO_ME_POLL_SEC = 0.3
|
|
# Arrival threshold: 0.32 size_ratio ≈ ~0.5m / arm's length.
|
|
# Patrol's 0.25 default is too lenient for come-to-me — it
|
|
# would treat a 1m-away user as 'already arrived'.
|
|
COME_TO_ME_ARRIVED_RATIO = 0.32
|
|
|
|
if not yolo_active:
|
|
# Fallback — same behaviour as before for hardware setups
|
|
# without YOLO loaded.
|
|
print(" [ComeToMe] YOLO not loaded — walking forward 2s")
|
|
execute_action("forward", 2.0)
|
|
resp = "Coming to you"
|
|
print(f"Sanad: {resp}")
|
|
add_to_history(cmd, resp)
|
|
log_cmd(cmd, resp)
|
|
return {"type": "move", "speak": resp, "action": "FORWARD 2.0s", "elapsed": 2.0}
|
|
|
|
print(f" [ComeToMe] tracking person up to {COME_TO_ME_MAX_SEC:.0f}s — say 'stop' to end")
|
|
deadline = time.time() + COME_TO_ME_MAX_SEC
|
|
last_log = 0.0
|
|
scan_attempts_left = 6 # how many turns before giving up if person never seen
|
|
while time.time() < deadline:
|
|
try:
|
|
from Core.motion_state import motion_abort
|
|
if motion_abort.is_set():
|
|
print(" [ComeToMe] motion_abort — stopping")
|
|
break
|
|
except Exception:
|
|
pass
|
|
try:
|
|
seen = bool(yolo_sees("person"))
|
|
except Exception:
|
|
seen = False
|
|
try:
|
|
# IMPORTANT: pass an explicit 'arrived' threshold (0.32)
|
|
# — without it, the default 0.25 (safety patrol's value)
|
|
# would call the user 'too close' when they're a metre
|
|
# away, and the robot would stop without walking. We
|
|
# want it to come arm's length close.
|
|
arrived = bool(yolo_person_too_close(
|
|
threshold=COME_TO_ME_ARRIVED_RATIO,
|
|
))
|
|
except Exception:
|
|
arrived = False
|
|
|
|
now = time.time()
|
|
if now - last_log > 1.5:
|
|
state = ("arrived (close to person)" if (seen and arrived)
|
|
else "tracking person" if seen
|
|
else "scanning for person")
|
|
print(f" [ComeToMe] {state}")
|
|
last_log = now
|
|
|
|
if seen and arrived:
|
|
# Made it — robot is close enough to stop in front
|
|
print(" [ComeToMe] arrived in front of person")
|
|
break
|
|
|
|
if seen and not arrived:
|
|
# Person is in view but not yet arm's length close —
|
|
# centre on them and walk forward in a burst.
|
|
bearing = "center"
|
|
try:
|
|
det = yolo_closest("person")
|
|
if det and getattr(det, "position", None):
|
|
# position is something like 'left' / 'center' / 'right'
|
|
bearing = str(det.position).lower()
|
|
except Exception:
|
|
pass
|
|
if bearing == "left":
|
|
execute_action("left", COME_TO_ME_TURN_SEC)
|
|
elif bearing == "right":
|
|
execute_action("right", COME_TO_ME_TURN_SEC)
|
|
# forward burst
|
|
move_step("forward", COME_TO_ME_STEP_SEC)
|
|
scan_attempts_left = 6 # reset
|
|
continue
|
|
|
|
# No person — try a small scan turn, then check again
|
|
if scan_attempts_left <= 0:
|
|
print(" [ComeToMe] no person found after scans — giving up")
|
|
break
|
|
scan_attempts_left -= 1
|
|
execute_action("left", COME_TO_ME_TURN_SEC)
|
|
time.sleep(COME_TO_ME_POLL_SEC)
|
|
|
|
try:
|
|
from API.zmq_api import gradual_stop as _gs
|
|
_gs()
|
|
except Exception:
|
|
pass
|
|
elapsed = time.time() - t0
|
|
resp = "Coming to you"
|
|
print(f"Sanad: {resp}")
|
|
add_to_history(cmd, resp)
|
|
log_cmd(cmd, resp)
|
|
return {"type": "move", "speak": resp, "action": "COME_TO_ME", "elapsed": elapsed}
|
|
|
|
# ── Multi-step compound ──────────────────────────────────────────────
|
|
_multi = re.match(
|
|
r"turn\s+(right|left)\s*(\d+)?\s*(?:deg(?:rees?)?)?\s+(?:and\s+then|then|and)?\s+"
|
|
r"(?:move\s+|go\s+|walk\s+|step\s+)?(back(?:ward)?|forward)\s*(\d+)?\s*(?:steps?|meter)?",
|
|
cmd, re.IGNORECASE)
|
|
if _multi:
|
|
turn_dir = _multi.group(1).lower()
|
|
turn_deg = float(_multi.group(2) or 90)
|
|
walk_dir = "backward" if "back" in _multi.group(3).lower() else "forward"
|
|
walk_dur = float(_multi.group(4) or 2)
|
|
execute_action("right" if turn_dir == "right" else "left", turn_deg / 18.0)
|
|
execute_action(walk_dir, walk_dur)
|
|
resp = f"Turned {turn_dir} {int(turn_deg)} degrees then moved {walk_dir}"
|
|
print(f"Sanad: {resp}")
|
|
add_to_history(cmd, resp)
|
|
log_cmd(cmd, resp)
|
|
return {"type": "move", "speak": resp, "action": f"MULTI {turn_dir}+{walk_dir}", "elapsed": time.time() - t0}
|
|
|
|
# ── Standard LLaVA command ───────────────────────────────────────────
|
|
return _handle_llava(cmd)
|
|
|
|
|
|
# ══════════════════════════════════════════════════════════════════════════════
|
|
# HANDLERS (return speak text)
|
|
# ══════════════════════════════════════════════════════════════════════════════
|
|
|
|
def _handle_search(cmd):
|
|
args = cmd[7:].strip()
|
|
if not args:
|
|
print(" Usage: search/ /path/to/photo.jpg [hint]")
|
|
return "Usage: search/ <path or hint>"
|
|
searcher = get_searcher()
|
|
if not searcher:
|
|
print(" [Search] Image search not available")
|
|
return "Image search not available"
|
|
parts = args.split(None, 1)
|
|
if parts and os.path.exists(parts[0]):
|
|
img_path = parts[0]
|
|
hint = parts[1].strip() if len(parts) > 1 else ""
|
|
yolo_pre = "person" if not hint or "person" in hint.lower() else None
|
|
log_cmd(cmd, f"Image search: {img_path}")
|
|
result = searcher.search_from_file(img_path, hint=hint, yolo_prefilter=yolo_pre)
|
|
return result.get("description", "Search complete")
|
|
hint = args
|
|
yolo_pre = "person" if any(w in hint.lower() for w in ("person", "guy", "man", "woman")) else None
|
|
log_cmd(cmd, f"Image search: {hint}")
|
|
result = searcher.search(ref_img_b64=None, hint=hint, yolo_prefilter=yolo_pre)
|
|
return result.get("description", "Search complete")
|
|
|
|
|
|
def _handle_talk(cmd):
|
|
print("Thinking...")
|
|
try:
|
|
img = get_frame()
|
|
facts_str = ""
|
|
try:
|
|
from API.llava_api import _facts
|
|
if _facts:
|
|
facts_str = "\nKnown facts: " + "; ".join(_facts) + "."
|
|
except ImportError:
|
|
pass
|
|
d = ask_talk(cmd, img, facts=facts_str)
|
|
sp = d.get("speak", "")
|
|
print(f"Sanad: {sp}")
|
|
log_cmd(cmd, sp)
|
|
return sp
|
|
except Exception as ex:
|
|
print(f" Error: {ex}")
|
|
return f"Error: {ex}"
|
|
|
|
|
|
def _handle_llava(cmd):
|
|
print("Thinking...")
|
|
t0 = time.time()
|
|
img = get_frame()
|
|
|
|
# Poll up to 500 ms in 50 ms slices instead of blocking a full second.
|
|
# Returns the moment a frame is available — most drops recover in <100 ms.
|
|
if img is None:
|
|
print(" Waiting for camera...")
|
|
for _ in range(10):
|
|
time.sleep(0.05)
|
|
img = get_frame()
|
|
if img is not None:
|
|
break
|
|
|
|
if img is None:
|
|
print(" Camera not ready — command cancelled")
|
|
log_cmd(cmd, "camera not ready")
|
|
return {"type": "error", "speak": "Camera not ready", "action": "NONE", "elapsed": 0}
|
|
|
|
d = ask(cmd, img)
|
|
dur = time.time() - t0
|
|
print(f" ({dur:.1f}s) -> {json.dumps(d)}")
|
|
resp = execute(d)
|
|
log_cmd(cmd, resp or "", dur)
|
|
|
|
from API.yolo_api import YOLO_AVAILABLE as _ya
|
|
if _ya:
|
|
for cls in yolo_all_classes():
|
|
det = yolo_closest(cls)
|
|
if det:
|
|
log_detection(cls, det.position, det.distance_estimate)
|
|
|
|
action_str = d.get("actions", [{}])[0].get("move", "NONE") if d.get("actions") else "NONE"
|
|
return {"type": "decision", "speak": resp or "", "action": action_str.upper(),
|
|
"elapsed": dur, "raw": d}
|
|
|
|
|
|
# ══════════════════════════════════════════════════════════════════════════════
|
|
# HELPERS
|
|
# ══════════════════════════════════════════════════════════════════════════════
|
|
|
|
def _strip_ansi(s: str) -> str:
|
|
"""Strip ANSI colour escapes for correct visual width calculations."""
|
|
import re as _re
|
|
return _re.sub(r"\x1b\[[0-9;]*m", "", s)
|
|
|
|
|
|
def get_brain_status() -> dict:
|
|
"""Return current brain status for server status message."""
|
|
from API.yolo_api import YOLO_AVAILABLE as _ya
|
|
from API.odometry_api import ODOM_AVAILABLE as _oa
|
|
from API.memory_api import MEMORY_AVAILABLE as _ma
|
|
from API.camera_api import CAM_WIDTH, CAM_HEIGHT, CAM_FPS
|
|
try:
|
|
from API.lidar_api import LIDAR_AVAILABLE as _la, get_loc_state
|
|
lidar_state = get_loc_state() if _la else "off"
|
|
except ImportError:
|
|
_la = False
|
|
lidar_state = "off"
|
|
return {
|
|
"model": OLLAMA_MODEL,
|
|
"yolo": _ya,
|
|
"odometry": _oa,
|
|
"memory": _ma,
|
|
"lidar": _la,
|
|
"lidar_state": lidar_state,
|
|
"voice": _voice_module is not None and _voice_module.is_running,
|
|
"camera": f"{CAM_WIDTH}x{CAM_HEIGHT}@{CAM_FPS}",
|
|
}
|
|
|
|
|
|
def shutdown():
|
|
"""Clean shutdown of all subsystems."""
|
|
print("\nShutting down Marcus...")
|
|
# Stop voice module
|
|
if _voice_module and _voice_module.is_running:
|
|
_voice_module.stop()
|
|
# Stop autonomous mode if running
|
|
from Brain.command_parser import _auto
|
|
if _auto and _auto.is_enabled():
|
|
_auto.disable()
|
|
stop_camera()
|
|
gradual_stop()
|
|
send_cmd("stop")
|
|
|
|
from API.odometry_api import odom as _o
|
|
if _o:
|
|
_o.stop()
|
|
|
|
from API.memory_api import mem as _m
|
|
if _m:
|
|
_m.end_session()
|
|
|
|
try:
|
|
from API.lidar_api import stop_lidar
|
|
stop_lidar()
|
|
except Exception:
|
|
pass
|
|
|
|
_log("Marcus stopped", "info", "brain")
|
|
print("Marcus stopped.")
|
|
|
|
|
|
# ══════════════════════════════════════════════════════════════════════════════
|
|
# TERMINAL MODE — used by run_marcus.py
|
|
# ══════════════════════════════════════════════════════════════════════════════
|
|
|
|
def run_terminal():
|
|
"""Run brain with terminal input loop."""
|
|
init_brain()
|
|
|
|
# ─── DASHBOARD ───────────────────────────────────────────────────────
|
|
# Separate the boot log from the interactive dashboard with a clear
|
|
# visual break. Print at the end of init so it's always the last thing
|
|
# on screen before the first `Command:` prompt — even if the operator
|
|
# scrolled through a wall of subsystem init messages.
|
|
status = get_brain_status()
|
|
|
|
def _fmt(v):
|
|
if v is True: return "\033[92mON \033[0m" # green
|
|
if v is False: return "\033[91mOFF\033[0m" # red
|
|
return str(v)
|
|
|
|
W = 58
|
|
LEFT_W = 28
|
|
RIGHT_W = (W - 2) - LEFT_W # visible chars available in the right column
|
|
|
|
def _pad(s: str, width: int) -> str:
|
|
"""ljust by visible width, treating ANSI colour escapes as zero-width."""
|
|
visible = len(_strip_ansi(s))
|
|
return s + " " * max(0, width - visible)
|
|
|
|
print("\n\n" + "╔" + "═" * (W-2) + "╗")
|
|
print("║" + _pad(" SANAD — AI BRAIN READY", W-2) + "║")
|
|
print("╠" + "═" * (W-2) + "╣")
|
|
from API.llava_api import VLM_ENABLED
|
|
left = [("model", status["model"]),
|
|
("vlm", _fmt(VLM_ENABLED)),
|
|
("voice", _fmt(status["voice"])),
|
|
("camera", status["camera"])]
|
|
right = [("yolo", _fmt(status["yolo"])),
|
|
("lidar", _fmt(status["lidar"])),
|
|
("memory", _fmt(status["memory"])),
|
|
("odometry", _fmt(status["odometry"]))]
|
|
for i in range(max(len(left), len(right))):
|
|
l = f" {left[i][0]:<8}: {left[i][1]}" if i < len(left) else ""
|
|
r = f" {right[i][0]:<8}: {right[i][1]}" if i < len(right) else ""
|
|
print("║" + _pad(l, LEFT_W) + _pad(r, RIGHT_W) + "║")
|
|
print("╠" + "═" * (W-2) + "╣")
|
|
print("║" + _pad(" Type a command, or say \"Sanad, <command>\".", W-2) + "║")
|
|
print("║" + _pad(" help · example · yolo · test_tts · auto on/off · q", W-2) + "║")
|
|
print("╚" + "═" * (W-2) + "╝\n")
|
|
|
|
try:
|
|
while True:
|
|
try:
|
|
cmd = input("Command: ").strip()
|
|
except (EOFError, KeyboardInterrupt):
|
|
break
|
|
if not cmd:
|
|
continue
|
|
if cmd.lower() in ("q", "quit", "exit"):
|
|
break
|
|
if cmd.lower() in ("mute/", "unmute/"):
|
|
# Route through the audio API so the action respects whichever
|
|
# mic backend is active (BuiltinMic flushes the UDP buffer;
|
|
# the legacy pactl path mutes PulseAudio source 3).
|
|
if _audio_api is None:
|
|
print(" Voice is not initialized")
|
|
continue
|
|
if cmd.lower() == "mute/":
|
|
_audio_api._mute_mic()
|
|
print(" Mic muted")
|
|
else:
|
|
_audio_api._unmute_mic()
|
|
print(" Mic unmuted")
|
|
continue
|
|
if cmd.lower().startswith("test_tts"):
|
|
# Probe speaker IDs to find which one speaks English on this
|
|
# firmware. Usage: `test_tts` (runs 0, 1, 2) or `test_tts 1`.
|
|
if _audio_api is None or _audio_api._tts_engine is None:
|
|
print(" Voice is not initialized")
|
|
continue
|
|
parts = cmd.split()
|
|
ids = [int(x) for x in parts[1:]] if len(parts) > 1 else [0, 1, 2]
|
|
phrase = "Hello, I am Sanad."
|
|
for sid in ids:
|
|
print(f" → speaker_id = {sid}")
|
|
_audio_api._tts_engine.speak(phrase, speaker_id=sid, block=True)
|
|
time.sleep(0.3)
|
|
print(' Pick the ID that sounded English and set it in')
|
|
print(' Config/config_Voice.json :: tts.builtin_speaker_id')
|
|
continue
|
|
# Structured motion log for terminal-typed commands. The
|
|
# voice path already logs through Voice/marcus_voice's
|
|
# worker; here we cover the OTHER entry-point so every
|
|
# motion (text or voice) shows up in logs/motion.log with
|
|
# parsed direction/magnitude/unit + actual elapsed.
|
|
try:
|
|
from Core.motion_log import log_motion as _mlog
|
|
_mlog("start", cmd, source="text")
|
|
_t0 = time.time()
|
|
except Exception:
|
|
_t0 = time.time()
|
|
result = process_command(cmd)
|
|
try:
|
|
_mlog("complete", cmd, source="text",
|
|
actual_sec=time.time() - _t0)
|
|
except Exception:
|
|
pass
|
|
sp = result.get("speak", "") if isinstance(result, dict) else ""
|
|
if sp and _audio_api:
|
|
_audio_api.speak(sp)
|
|
except KeyboardInterrupt:
|
|
pass
|
|
|
|
shutdown()
|
|
|
|
|
|
if __name__ == "__main__":
|
|
run_terminal()
|