361 lines
14 KiB
Python
361 lines
14 KiB
Python
"""
|
|
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""")
|