""" command_parser.py — Local command regex patterns + dispatcher Handles place memory, odometry, session recall, help, examples """ import re import time from API.zmq_api import send_vel, gradual_stop from API.memory_api import mem, place_save, place_goto, places_list_str from API.odometry_api import odom, ODOM_AVAILABLE from API.camera_api import get_frame from API.llava_api import ask from Brain.executor import execute # ── Compiled patterns ──────────────────────────────────────────────────────── _RE_REMEMBER = re.compile( r"^(?:remember|save|mark|call|name|label)\s+(?:this|here|current|position)?\s*as\s+(.+)$", re.I) _RE_GOTO = re.compile( r"^(?:go\s+to|navigate\s+to|take\s+me\s+to|move\s+to|return\s+to|head\s+to)\s+(.+)$", re.I) _RE_FORGET = re.compile( r"^(?:forget|delete|remove)\s+(?:place\s+)?(.+)$", re.I) _RE_RENAME = re.compile( r"^rename\s+(.+?)\s+(?:to|as)\s+(.+)$", re.I) _RE_WALK_DIST = re.compile( r"^(?:walk|go|move)\s+(?:forward\s+)?(\d+(?:\.\d+)?)\s*m(?:eter(?:s)?)?$", re.I) _RE_WALK_BACK = re.compile( r"^(?:walk|go|move)\s+backward?\s+(\d+(?:\.\d+)?)\s*m(?:eter(?:s)?)?$", re.I) _RE_TURN_DEG = re.compile( r"^turn\s+(?:(left|right)\s+)?(\d+(?:\.\d+)?)\s*deg(?:ree(?:s)?)?$", re.I) _RE_PATROL_RT = re.compile( r"^patrol[/:]\s*(.+)$", re.I) _RE_LAST_CMD = re.compile( r"^(?:last\s+command|what\s+did\s+i\s+(?:say|type)\s+last|repeat\s+last)$", re.I) _RE_DO_AGAIN = re.compile( r"^(?:do\s+that\s+again|repeat|again|redo)$", re.I) _RE_UNDO = re.compile( r"^(?:undo|go\s+back\s+(?:to\s+)?(?:where|from\s+where)\s+(?:you|i)\s+(?:started|were|came)|reverse\s+last|turn\s+back\s+from).*$", re.I) _RE_LAST_SESS = re.compile( r"^(?:last\s+session|what\s+(?:did\s+you\s+do|happened)\s+last\s+(?:session|time)|previous\s+session)$", re.I) _RE_WHERE = re.compile( r"^(?:where\s+am\s+i|current\s+position|my\s+(?:location|position)|position)$", re.I) _RE_GO_HOME = re.compile( r"^(?:go\s+home|return\s+to\s+start|come\s+back\s+home|go\s+back\s+to\s+start)$", re.I) _RE_SESSION_SUMMARY = re.compile( r"^(?:session\s+summary|what\s+happened\s+today|session\s+report)$", re.I) _RE_AUTO = re.compile( r"^auto(?:nomous)?\s+(on|off|status|save|summary)$", re.I) # Autonomous mode instance — set by init_autonomous() _auto = None def init_autonomous(auto_instance): """Wire in the AutonomousMode instance from marcus_brain.""" global _auto _auto = auto_instance def try_local_command(cmd: str) -> bool: """ Handle local commands (place, odom, memory, help). Returns True if handled, False if not matched (send to LLaVA). """ # ── PLACE MEMORY ───────────────────────────────────────────────────── m = _RE_REMEMBER.match(cmd) if m: place_save(m.group(1).strip()) return True m = _RE_GOTO.match(cmd) if m: name = m.group(1).strip() if name.lower() in ("start", "home", "beginning"): if odom and ODOM_AVAILABLE: odom.return_to_start() else: print(" [Places] Odometry not running — cannot return to start") else: place_goto(name) return True m = _RE_FORGET.match(cmd) if m: if mem: mem.delete_place(m.group(1).strip()) else: print(" [Places] Memory not available") return True m = _RE_RENAME.match(cmd) if m: if mem: mem.rename_place(m.group(1).strip(), m.group(2).strip()) else: print(" [Places] Memory not available") return True if re.match(r"^(?:places|list\s+places|what\s+places|show\s+(?:places|locations)|known\s+places)$", cmd, re.I): print(places_list_str()) return True # ── ODOMETRY MOVEMENT ──────────────────────────────────────────────── m = _RE_WALK_DIST.match(cmd) if m: meters = float(m.group(1)) if odom: odom.walk_distance(meters) else: t0 = time.time() while time.time() - t0 < meters / 0.3: send_vel(vx=0.3) time.sleep(0.05) gradual_stop() return True m = _RE_WALK_BACK.match(cmd) if m: meters = float(m.group(1)) if odom: odom.walk_distance(meters, direction="backward") else: t0 = time.time() while time.time() - t0 < meters / 0.2: send_vel(vx=-0.2) time.sleep(0.05) gradual_stop() return True m = _RE_TURN_DEG.match(cmd) if m: direction = m.group(1) degrees = float(m.group(2)) if direction and direction.lower() == "right": degrees = -degrees if odom: odom.turn_degrees(degrees) else: t0 = time.time() vyaw = 0.3 if degrees > 0 else -0.3 duration = abs(degrees) / 17.2 while time.time() - t0 < duration: send_vel(vyaw=vyaw) time.sleep(0.05) gradual_stop() return True # ── NAMED PATROL ROUTE ─────────────────────────────────────────────── m = _RE_PATROL_RT.match(cmd) if m: raw_route = m.group(1) names = re.split(r"[→,;]+|\s{2,}", raw_route) names = [n.strip() for n in names if n.strip()] if not names: print(" Usage: patrol: door → desk → exit") return True if not mem: print(" [Places] Memory not available") return True waypoints, missing = [], [] for name in names: place = mem.get_place(name) if place is None: missing.append(name) elif not place.get("has_odom"): print(f" [Places] '{name}' has no coordinates — skipping") else: waypoints.append({"x": place["x"], "y": place["y"], "heading": place["heading"], "name": name}) if missing: print(f" [Places] Unknown places: {', '.join(missing)}") if not waypoints: print(" [Places] No valid waypoints — patrol cancelled") return True if odom: print(f" [Places] Named patrol: {' → '.join(n['name'] for n in waypoints)}") odom.patrol_route(waypoints) else: print(" [Places] Odometry not running") return True # ── SESSION MEMORY RECALL ──────────────────────────────────────────── if _RE_LAST_CMD.match(cmd): if mem: last = mem.get_last_command() print(f" Last command: '{last}'" if last else " No commands logged yet") else: print(" Memory not available") return True if _RE_UNDO.match(cmd): if not mem: print(" Memory not available — cannot undo") return True recent = mem.get_last_n_commands(5) move_words = {"turn right": ("left", 1), "turn left": ("right", -1), "walk forward": ("backward", 1), "move forward": ("backward", 1), "move back": ("forward", 1), "walk backward": ("forward", 1)} for c in reversed(recent): cl = c.lower() for phrase, (reverse_dir, _) in move_words.items(): if phrase in cl: print(f" Undoing: '{c}' → reversing with '{reverse_dir}'") dur, t0 = 2.0, time.time() if reverse_dir in ("left", "right"): vyaw = 0.3 if reverse_dir == "left" else -0.3 while time.time() - t0 < dur: send_vel(vyaw=vyaw) time.sleep(0.05) else: vx = 0.3 if reverse_dir == "forward" else -0.2 while time.time() - t0 < dur: send_vel(vx=vx) time.sleep(0.05) gradual_stop() return True print(" No movement command to undo") return True if _RE_DO_AGAIN.match(cmd): if not mem: print(" Memory not available — cannot repeat") return True recent = mem.get_last_n_commands(5) repeat = None for c in reversed(recent): if not _RE_DO_AGAIN.match(c) and not _RE_LAST_CMD.match(c): repeat = c break if repeat: print(f" Repeating: '{repeat}'") if try_local_command(repeat): return True # Not a local command — send directly to LLaVA print("Thinking...") img = get_frame() if img: d = ask(repeat, img) execute(d) return True else: print(" No previous command to repeat") return True if _RE_LAST_SESS.match(cmd): if mem: print(mem.last_session_summary()) else: print(" Memory not available") return True if _RE_SESSION_SUMMARY.match(cmd): if mem: print(f" Session: {mem._session_id}") print(f" Duration: {mem.session_duration_str()}") print(f" Commands: {mem.commands_count()}") print(f" Places: {mem.places_count()}") detections = mem.get_session_detections() classes = {d.get("class") for d in detections} print(f" Detected: {', '.join(classes) if classes else 'nothing yet'}") else: print(" Memory not available") return True if _RE_WHERE.match(cmd): if odom and ODOM_AVAILABLE: print(f" Position: {odom.status_str()}") print(f" Distance from start: {odom.get_distance_from_start():.2f}m") else: print(" Odometry not running — position unknown") return True if _RE_GO_HOME.match(cmd): if odom and ODOM_AVAILABLE: odom.return_to_start() else: print(" Odometry not running — cannot navigate home") return True # ── AUTONOMOUS MODE ────────────────────────────────────────────────── m = _RE_AUTO.match(cmd) if m: subcmd = m.group(1).lower() if _auto is None: print(" [Auto] Autonomous mode not initialized") return True if subcmd == "on": _auto.enable() elif subcmd == "off": _auto.disable() elif subcmd == "status": _auto.status() elif subcmd == "save": _auto.save_snapshot() elif subcmd == "summary": if _auto.is_enabled(): _auto.status() else: print(" [Auto] Not running — use 'auto on' to start") return True # ── LIDAR STATUS ───────────────────────────────────────────────────── if re.match(r"^(?:lidar|lidar\s+status|slam\s+status)$", cmd, re.I): try: from API.lidar_api import LIDAR_AVAILABLE, get_lidar_status if not LIDAR_AVAILABLE: print(" LiDAR: not available") else: s = get_lidar_status() print(f" LiDAR: {s['mode']} | loc: {s['loc_state']} | " f"frame age: {s['last_frame_age']}s") if s.get("pose"): p = s["pose"] print(f" SLAM pose: x={p['x']:.2f} y={p['y']:.2f} h={p['heading']:.1f}") safety = s.get("safety", {}) if safety.get("emergency"): print(f" EMERGENCY: {safety.get('reasons', [])}") perf = s.get("perf", {}) if perf: print(f" FPS: {perf.get('input_fps', 0):.0f} in / " f"ICP: {perf.get('icp_ms', 0):.1f}ms / " f"CPU: {perf.get('cpu_percent', 0):.0f}%") except ImportError: print(" LiDAR: module not loaded") return True # ── HELP / EXAMPLES ────────────────────────────────────────────────── if re.match(r"^(?:help[/]|help|commands|menu|[?][/]|[?])$", cmd, re.I): _print_help() return True if re.match(r"^(?:example[/]|examples[/]|ex[/]|example|examples|ex|show examples)$", cmd, re.I): _print_examples() return True return False def _print_help(): print(""" MARCUS — COMMAND HELP Movement: turn left/right, walk forward/back, walk 1 meter, turn 90 degrees Vision: what do you see, yolo Goals: goal/ stop when you see a person Places: remember this as door, go to door, places, forget door Patrol: patrol, patrol: door → desk → exit Session: last command, do that again, last session, session summary Search: search/ /path/to/photo.jpg [hint], search/ person in blue Auto: auto on, auto off, auto status, auto save, auto summary LiDAR: lidar, lidar status System: help, example, yolo, q""") def _print_examples(): print(""" MARCUS — USAGE EXAMPLES turn left | turn right 90 degrees | walk forward | walk 1 meter what do you see | describe what is in front of you goal/ stop when you see a person | goal/ stop when you see a laptop remember this as door | go to door | places | forget door patrol | patrol: door → desk → window last command | do that again | last session | session summary""")