""" 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, MOVE_MAP, STEP_DURATION_SEC 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) # Step-based motion: "walk 1 step", "walk forward 2 steps", "move back 1 step", # "turn left", "turn right 2 steps". Kept local so these never fall through to # the VLM — on the Jetson the cold-load is 60-90 s and we don't want to pay # that for a trivial two-second motion. One step = 2 s of motion at the default # velocity, matching the undo-loop duration already used below. _RE_WALK_STEP = re.compile( r"^(?:walk|go|move|step)(?:\s+(forward|back(?:ward)?))?\s+(\d+)\s*steps?$", re.I) _RE_TURN_STEP = re.compile( r"^turn\s+(left|right)(?:\s+(\d+)\s*steps?)?$", re.I) # Simple one-shot motion — one word or verb+direction, no counts/units. # All default to a ~2 s motion at the normal velocity. Kept local so the # user doesn't eat a 5 s Qwen round-trip for a trivial "go back". # # Matches: # "left" / "right" / "forward" / "back" / "backward" # "go back" / "step back" / "move back" / "walk back" / "run back" # "go forward" / "step forward" / "move forward" / "walk forward" # "go left" / "move right" / "step left" / etc. # "head forward" / "head back" # Does NOT match multi-word phrases like "walk to the chair" — those # still fall through to Qwen where they belong. _RE_SIMPLE_DIR = re.compile( r"^(?:(?:walk|go|move|step|run|head)\s+)?" r"(forward|back(?:ward)?|left|right)$", re.I, ) # Bare stop / pause words — no need to ask Qwen what "stop" means. _RE_STOP_SIMPLE = re.compile( r"^(?:stop|halt|wait|pause|stay|freeze|hold|stand\s+still|don'?t\s+move)$", 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: vx, _, _ = MOVE_MAP["forward"] t0 = time.time() while time.time() - t0 < meters / abs(vx): send_vel(vx=vx) 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: vx, _, _ = MOVE_MAP["backward"] t0 = time.time() while time.time() - t0 < meters / abs(vx): send_vel(vx=vx) 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: # vyaw magnitude comes from MOVE_MAP["left"]; duration is # abs(degrees)/(vyaw_deg_per_sec). vyaw in config is rad/s. _, _, vyaw_mag = MOVE_MAP["left"] vyaw_deg_per_sec = abs(vyaw_mag) * 180.0 / 3.14159265 vyaw = vyaw_mag if degrees > 0 else -vyaw_mag duration = abs(degrees) / vyaw_deg_per_sec t0 = time.time() while time.time() - t0 < duration: send_vel(vyaw=vyaw) time.sleep(0.05) gradual_stop() return True m = _RE_WALK_STEP.match(cmd) if m: direction = (m.group(1) or "forward").lower() if direction.startswith("back"): direction = "backward" steps = int(m.group(2)) vx, _, _ = MOVE_MAP[direction] duration = STEP_DURATION_SEC * steps t0 = time.time() while time.time() - t0 < duration: send_vel(vx=vx) time.sleep(0.05) gradual_stop() return True m = _RE_TURN_STEP.match(cmd) if m: direction = m.group(1).lower() steps = int(m.group(2)) if m.group(2) else 1 _, _, vyaw = MOVE_MAP[direction] duration = STEP_DURATION_SEC * steps t0 = time.time() while time.time() - t0 < duration: send_vel(vyaw=vyaw) time.sleep(0.05) gradual_stop() return True # ── BARE / SIMPLE DIRECTIONAL COMMANDS ─────────────────────────────── # "left", "right", "forward", "back", "go back", "move forward", # "step right", "walk back", "head forward" — any one-word direction # or verb+direction with no explicit count. Duration and velocities # come entirely from config_Navigation.json (MOVE_MAP + # step_duration_sec) — no magic numbers here. m = _RE_SIMPLE_DIR.match(cmd) if m: direction = m.group(1).lower() if direction.startswith("back"): direction = "backward" vx, vy, vyaw = MOVE_MAP[direction] t0 = time.time() while time.time() - t0 < STEP_DURATION_SEC: send_vel(vx=vx, vy=vy, vyaw=vyaw) time.sleep(0.05) gradual_stop() return True # ── BARE STOP / PAUSE ───────────────────────────────────────────────── m = _RE_STOP_SIMPLE.match(cmd) if m: 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}'") vx, vy, vyaw = MOVE_MAP[reverse_dir] t0 = time.time() while time.time() - t0 < STEP_DURATION_SEC: send_vel(vx=vx, vy=vy, vyaw=vyaw) 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""")