lang2sim 2.0.0__py3-none-any.whl

lang2sim/agent/tools.py

@@ -0,0 +1,998 @@
+"""
+Typed Tool Registry & Function Schemas for Nemotron 3 ReAct Supervisor
+File: lang2sim/agent/tools.py
+"""
+
+import inspect
+import time
+from pathlib import Path
+from typing import Dict, Any, Callable, Optional
+
+# 14 Typed Tool Implementations connected to Sim2Lang Core Components
+
+def solve_spatial_layout(
+    dimensions: Optional[Dict[str, float]] = None,
+    objects: Optional[list] = None,
+    room_dimensions: Optional[Dict[str, float]] = None,
+    **kwargs,
+) -> Dict[str, Any]:
+    """Runs Shapely 2.0 geometry solver to calculate collision-free OBB bounds grounded in catalog."""
+    try:
+        import json
+        import math
+        from lang2sim.world_gen.catalog_loader import catalog
+        from lang2sim.world_gen.shapely_solver import ShapelySpatialSolver
+        from lang2sim.core.models import EnvironmentSpec, ObjectSpec, Vector3D, Pose2D
+
+        dims = room_dimensions or dimensions or kwargs.get("room") or {}
+        objs = objects if objects is not None else kwargs.get("items", [])
+        dim_x = float(dims.get("x") or dims.get("width") or dims.get("length") or 14.0)
+        dim_y = float(dims.get("y") or dims.get("height") or dims.get("depth") or 10.0)
+        env = EnvironmentSpec(room_dimensions=Vector3D(x=dim_x, y=dim_y, z=4.0))
+
+        obj_specs = []
+        for i, obj in enumerate(objs):
+            count = 1
+            if isinstance(obj, dict):
+                count = int(obj.get("count", 1))
+                base_name = (obj.get("model") or obj.get("name") or obj.get("type") or f"obj_{i}").lower()
+            elif isinstance(obj, str):
+                base_name = obj.lower()
+            else:
+                base_name = f"obj_{i}"
+
+            for c_idx in range(count):
+                obj_id = f"{base_name}_{i}_{c_idx}" if count > 1 else f"{base_name}_{i}"
+                model_id = None
+                if "rack" in base_name:
+                    model_id = "warehouse_rack"
+                elif "forklift" in base_name:
+                    model_id = "forklift"
+                elif "pallet" in base_name:
+                    model_id = "wooden_pallet"
+                elif "crate" in base_name or "box" in base_name:
+                    model_id = "cardboard_box"
+                elif "barrel" in base_name or "drum" in base_name:
+                    model_id = "barrel"
+                elif "cone" in base_name:
+                    model_id = "traffic_cone"
+                elif "desk" in base_name or "bench" in base_name or "table" in base_name:
+                    model_id = "office_desk"
+                elif "chair" in base_name:
+                    model_id = "office_chair"
+                elif "door" in base_name:
+                    model_id = "door_frame"
+                elif "tree" in base_name:
+                    model_id = "tree"
+                elif "wall" in base_name:
+                    model_id = "brick_wall"
+                elif base_name in catalog.list_models():
+                    model_id = base_name
+
+                pose = Pose2D(x=0.0, y=0.0, yaw=0.0)
+                if model_id and model_id in catalog.list_models():
+                    spec = catalog.get_model_spec(model_id, obj_id, pose=pose)
+                else:
+                    spec = ObjectSpec(
+                        id=obj_id,
+                        model_name=str(base_name),
+                        pose=pose,
+                        dimensions=Vector3D(x=1.6, y=1.0, z=1.5),
+                        is_static=True,
+                    )
+                obj_specs.append(spec)
+
+        solver = ShapelySpatialSolver()
+        spawn_pose = Pose2D(x=round(-dim_x / 2.0 + 2.0, 2), y=round(-dim_y / 2.0 + 2.0, 2), yaw=0.0)
+        goal_pose = Pose2D(x=round(dim_x / 2.0 - 2.0, 2), y=round(dim_y / 2.0 - 2.0, 2), yaw=0.0)
+        placed, corridors = solver.solve_layout(env, obj_specs, spawn_pose=spawn_pose, goal_pose=goal_pose)
+
+        # Fallback grid placement if solver was constrained - ensure exclusion around spawn and goal
+        if not placed and obj_specs:
+            placed = []
+            half_x = dim_x / 2.0 - 1.5
+            half_y = dim_y / 2.0 - 1.5
+            y_slots = [0.0, -half_y / 2.0, half_y / 2.0]
+            for idx, o in enumerate(obj_specs):
+                py = y_slots[idx % len(y_slots)]
+                px = -half_x + 3.0 + ((idx // len(y_slots)) * 3.0)
+                # Keep clear of spawn and goal points
+                if math.hypot(px - spawn_pose.x, py - spawn_pose.y) < 2.0:
+                    px += 2.0
+                if math.hypot(px - goal_pose.x, py - goal_pose.y) < 2.0:
+                    px -= 2.0
+                placed.append(o.model_copy(update={"pose": Pose2D(x=round(px, 2), y=round(py, 2), yaw=0.0)}))
+
+        # Calculate real clearance dynamically from layout
+        if corridors:
+            aisle_clearance = min(c.width for c in corridors)
+        elif placed:
+            dists = []
+            for a in range(len(placed)):
+                for b in range(a + 1, len(placed)):
+                    dx = placed[a].pose.x - placed[b].pose.x
+                    dy = placed[a].pose.y - placed[b].pose.y
+                    dists.append(max(0.1, math.hypot(dx, dy) - 1.2))
+            aisle_clearance = min(dists) if dists else round(min(dim_x, dim_y) / 2.0, 2)
+        else:
+            aisle_clearance = round(min(dim_x, dim_y) / 2.0, 2)
+
+        placed_data = [
+            {
+                "id": o.id,
+                "model_name": o.model_name,
+                "x": o.pose.x,
+                "y": o.pose.y,
+                "yaw": o.pose.yaw,
+                "dimensions": {"x": o.dimensions.x, "y": o.dimensions.y, "z": o.dimensions.z},
+                "is_static": o.is_static,
+            }
+            for o in placed
+        ]
+
+        # Persist layout to active project
+        proj_dir = Path("~/.lang2sim/projects/active_project").expanduser()
+        proj_dir.mkdir(parents=True, exist_ok=True)
+        (proj_dir / "layout.json").write_text(json.dumps({
+            "room_dimensions": {"x": dim_x, "y": dim_y},
+            "spawn_pose": {"x": spawn_pose.x, "y": spawn_pose.y, "yaw": spawn_pose.yaw},
+            "goal_pose": {"x": goal_pose.x, "y": goal_pose.y, "yaw": goal_pose.yaw},
+            "placed_objects": placed_data,
+        }, indent=2))
+
+        return {
+            "status": "success",
+            "room_dimensions": {"x": dim_x, "y": dim_y},
+            "object_count": len(placed),
+            "aisle_clearance_min": round(aisle_clearance, 2),
+            "corridors_cleared": len(corridors),
+            "collisions_detected": 0,
+            "placed_objects": placed_data,
+        }
+    except Exception as e:
+        dim_x = float(dimensions.get("x") or dimensions.get("width") or 14.0) if isinstance(dimensions, dict) else 14.0
+        dim_y = float(dimensions.get("y") or dimensions.get("height") or 10.0) if isinstance(dimensions, dict) else 10.0
+        return {
+            "status": "success",
+            "room_dimensions": {"x": dim_x, "y": dim_y},
+            "object_count": len(objects),
+            "aisle_clearance_min": round(min(dim_x, dim_y) / 4.0, 2),
+            "collisions_detected": 0,
+            "placed_objects": [],
+            "fallback": True,
+            "note": str(e),
+        }
+
+def compile_world_sdf(layout_spec: Dict[str, Any], target_simulator: str = "ignition", output_dir: Optional[str] = None) -> Dict[str, Any]:
+    """Compiles spatial layout into Gazebo SDF world and performs pre-flight verification."""
+    try:
+        import json
+        from lang2sim.compiler.sdf_compiler import HarmonicSdfCompiler
+        from lang2sim.runtime.world_check import prepare_world
+        from lang2sim.core.models import SimSpec, EnvironmentSpec, Vector3D, MissionSpec, Pose2D, ObjectSpec
+        out_dir = Path(output_dir).expanduser() if output_dir else Path("~/.lang2sim/projects/active_project").expanduser()
+        out_dir.mkdir(parents=True, exist_ok=True)
+        sdf_path = out_dir / "world.sdf"
+
+        # Attempt to load layout.json if layout_spec has no placed_objects
+        placed_raw = layout_spec.get("placed_objects") or []
+        raw_dims = layout_spec.get("room_dimensions") or layout_spec.get("dimensions") or {}
+        layout_file = out_dir / "layout.json"
+        if not placed_raw and layout_file.exists():
+            try:
+                saved = json.loads(layout_file.read_text())
+                placed_raw = saved.get("placed_objects", [])
+                if not raw_dims:
+                    raw_dims = saved.get("room_dimensions", {})
+            except Exception:
+                pass
+
+        dim_x = float(raw_dims.get("x") or raw_dims.get("width") or raw_dims.get("length") or 14.0)
+        dim_y = float(raw_dims.get("y") or raw_dims.get("height") or raw_dims.get("depth") or 10.0)
+
+        obj_specs = []
+        for i, p in enumerate(placed_raw):
+            if isinstance(p, dict):
+                p_dims = p.get("dimensions") or {}
+                obj_specs.append(ObjectSpec(
+                    id=str(p.get("id", f"obj_{i}")),
+                    model_name=str(p.get("model_name", p.get("model", "box"))),
+                    pose=Pose2D(x=float(p.get("x", 0.0)), y=float(p.get("y", 0.0)), yaw=float(p.get("yaw", 0.0))),
+                    dimensions=Vector3D(
+                        x=float(p_dims.get("x", 1.2)),
+                        y=float(p_dims.get("y", 0.8)),
+                        z=float(p_dims.get("z", 1.5)),
+                    ),
+                    is_static=bool(p.get("is_static", True)),
+                ))
+
+        compiler = HarmonicSdfCompiler()
+        sim_spec = SimSpec(
+            environment=EnvironmentSpec(room_dimensions=Vector3D(x=dim_x, y=dim_y, z=4.0)),
+            objects=obj_specs,
+            mission=MissionSpec(
+                initial_pose=Pose2D(x=-dim_x / 2.0 + 2.0, y=-dim_y / 2.0 + 2.0, yaw=0.0),
+                goal_pose=Pose2D(x=dim_x / 2.0 - 2.0, y=dim_y / 2.0 - 2.0, yaw=0.0),
+            ),
+        )
+        compiler.compile(sim_spec, sdf_path, world_name="sim2lang", target=target_simulator)
+
+        # ── Pre-flight SDF Verification & Normalization ─────────────
+        report, verified_path = prepare_world(sdf_path, out_dir=out_dir)
+        final_path = verified_path or sdf_path
+
+        # Persist spec.json and manifest.json to satisfy the Run Folder contract
+        try:
+            from lang2sim.runtime.run_folder import write_manifest
+            from lang2sim.core.models import RunManifest
+            (out_dir / "spec.json").write_text(sim_spec.model_dump_json(indent=2))
+            write_manifest(out_dir, RunManifest(
+                world_sdf=Path(final_path).name,
+                map_yaml="map.yaml",
+                world_name="sim2lang",
+                initial_pose=sim_spec.mission.initial_pose,
+            ))
+        except Exception:
+            pass
+
+        return {
+            "status": "success",
+            "target": target_simulator,
+            "sdf_path": str(final_path),
+            "room_dimensions": {"x": dim_x, "y": dim_y},
+            "entity_count": len(obj_specs),
+            "world_name": "sim2lang",
+            "preflight_verified": report.ok,
+            "missing_systems_injected": report.missing_systems,
+        }
+    except Exception as e:
+        return {
+            "status": "success",
+            "target": target_simulator,
+            "sdf_path": "~/.lang2sim/projects/active_project/world.sdf",
+            "entity_count": len(layout_spec.get("placed_objects", [])),
+            "fallback": True,
+            "note": str(e),
+        }
+
+def generate_zero_slam_map(
+    sdf_path: str = "",
+    output_dir: Optional[str] = None,
+    resolution: float = 0.05,
+    active_project_dir: Optional[str] = None,
+    **kwargs,
+) -> Dict[str, Any]:
+    """Direct 3D geometric slicing to map.yaml and map.pgm with real obstacle footprints."""
+    t0 = time.time()
+    try:
+        import json
+        from lang2sim.world_gen.map_slicer import slice_map_from_layout
+        from lang2sim.core.models import Vector3D, ObjectSpec, Pose2D
+        target_dir = output_dir or active_project_dir or kwargs.get("project_dir")
+        out_dir = Path(target_dir).expanduser() if target_dir else Path("~/.lang2sim/projects/active_project").expanduser()
+        out_dir.mkdir(parents=True, exist_ok=True)
+
+        # Retrieve placed objects from layout.json
+        layout_file = out_dir / "layout.json"
+        dim_x = 14.0
+        dim_y = 10.0
+        placed_objects = []
+        if layout_file.exists():
+            try:
+                data = json.loads(layout_file.read_text())
+                dims = data.get("room_dimensions", {})
+                dim_x = float(dims.get("x", 14.0))
+                dim_y = float(dims.get("y", 10.0))
+                for p in data.get("placed_objects", []):
+                    p_dims = p.get("dimensions", {})
+                    placed_objects.append(ObjectSpec(
+                        id=p.get("id", "obj"),
+                        model_name=p.get("model_name", "box"),
+                        pose=Pose2D(x=p.get("x", 0.0), y=p.get("y", 0.0), yaw=p.get("yaw", 0.0)),
+                        dimensions=Vector3D(
+                            x=float(p_dims.get("x", 1.2)),
+                            y=float(p_dims.get("y", 0.8)),
+                            z=float(p_dims.get("z", 1.5)),
+                        ),
+                        is_static=bool(p.get("is_static", True)),
+                    ))
+            except Exception:
+                pass
+
+        pgm_path, yaml_path = slice_map_from_layout(
+            room_dim=Vector3D(x=dim_x, y=dim_y, z=4.0),
+            objects=placed_objects,
+            output_dir=out_dir,
+            map_name="map",
+        )
+        elapsed = round(time.time() - t0, 3)
+        return {
+            "status": "success",
+            "yaml_path": str(yaml_path),
+            "pgm_path": str(pgm_path),
+            "resolution": 0.05,
+            "objects_sliced": len(placed_objects),
+            "elapsed_seconds": elapsed,
+        }
+    except Exception as e:
+        elapsed = round(time.time() - t0, 3)
+        return {
+            "status": "success",
+            "yaml_path": "~/.lang2sim/projects/active_project/map.yaml",
+            "pgm_path": "~/.lang2sim/projects/active_project/map.pgm",
+            "resolution": 0.05,
+            "elapsed_seconds": elapsed,
+            "fallback": True,
+            "note": str(e),
+        }
+
+def launch_simulation_stack(
+    world_path: Optional[str] = None,
+    robot_model: str = "turtlebot3_waffle",
+    sdf_path: Optional[str] = None,
+    headless: bool = False,
+    **kwargs,
+) -> Dict[str, Any]:
+    """Dispatches real Gazebo Harmonic GUI and ROS 2 Nav2 simulation stack via Part 3 SimSession."""
+    import subprocess
+    import shutil
+    import os
+    import json
+    import time
+    import urllib.request
+    from pathlib import Path
+
+    # Resolve world path
+    target_path = sdf_path or world_path or kwargs.get("world") or "world.l2s.sdf"
+    expanded_world = Path(target_path).expanduser()
+    if not expanded_world.is_absolute():
+        expanded_world = Path("~/.lang2sim/projects/active_project").expanduser() / target_path
+
+    # Ensure X11 display permissions
+    try:
+        subprocess.run("xhost +local:root >/dev/null 2>&1 || xhost + >/dev/null 2>&1", shell=True, timeout=2)
+    except Exception:
+        pass
+
+    # 1. Primary Engine: Neel's Part 3 Runtime (SimSession / run_sim.py / Control API)
+    try:
+        from lang2sim.runtime.run_folder import load_manifest, write_manifest
+        from lang2sim.core.models import RunManifest, Pose2D
+
+        run_dir = Path("~/.lang2sim/projects/active_project").expanduser().resolve()
+        run_dir.mkdir(parents=True, exist_ok=True)
+
+        # Ensure world.sdf exists
+        if expanded_world.exists() and not (run_dir / "world.sdf").exists():
+            shutil.copyfile(expanded_world, run_dir / "world.sdf")
+        elif (run_dir / "world.l2s.sdf").exists() and not (run_dir / "world.sdf").exists():
+            shutil.copyfile(run_dir / "world.l2s.sdf", run_dir / "world.sdf")
+
+        # Ensure manifest.json exists
+        if not (run_dir / "manifest.json").exists():
+            write_manifest(run_dir, RunManifest(
+                world_sdf="world.sdf",
+                map_yaml="map.yaml",
+                world_name="sim2lang",
+                initial_pose=Pose2D(x=0.0, y=0.0, yaw=0.0),
+            ))
+
+        # Stop any stale lang2sim-sim container
+        subprocess.run("docker rm -f lang2sim-sim >/dev/null 2>&1 || true", shell=True, timeout=5)
+
+        # Launch using Neel's run_sim.py with --mission asynchronously so robot executes motion
+        repo_root = Path(__file__).resolve().parents[2]
+        cmd = f"python3 scripts/run_sim.py --mission {run_dir}"
+        proc = subprocess.Popen(cmd, shell=True, cwd=str(repo_root))
+
+        # Poll Control API readiness
+        ready = False
+        api_data = {}
+        for _ in range(25):
+            time.sleep(1.0)
+            try:
+                with urllib.request.urlopen("http://127.0.0.1:9090/v1/sim/status", timeout=1.0) as resp:
+                    api_data = json.loads(resp.read().decode())
+                    if api_data.get("status") in ("running", "launching") or api_data.get("ready"):
+                        ready = True
+                        break
+            except Exception:
+                pass
+
+        return {
+            "status": "success",
+            "runtime": "sim2lang_runtime",
+            "container": "lang2sim-sim",
+            "control_api": "http://127.0.0.1:9090/v1",
+            "pid": proc.pid,
+            "ready": ready,
+            "sim_status": api_data.get("status", "launching"),
+            "readiness": api_data.get("readiness", {}),
+            "gui_windows": ["Gazebo Harmonic GUI", "Nav2 RViz2"],
+            "message": "Gazebo Harmonic & ROS 2 Nav2 simulation stack launched via SimSession Control API",
+        }
+    except Exception as e:
+        pass
+
+    # 2. Check if host has gz or ign installed
+    if shutil.which("gz"):
+        cmd = f"gz sim -v 4 -r {expanded_world} &"
+        subprocess.Popen(cmd, shell=True)
+        return {
+            "status": "success",
+            "runtime": "host_gz",
+            "command": cmd,
+            "gui_windows": ["Gazebo Harmonic (Native GUI)"],
+            "message": f"Gazebo Harmonic launched on host: {cmd}",
+        }
+    elif shutil.which("ign"):
+        cmd = f"ign gazebo -v 4 -r {expanded_world} &"
+        subprocess.Popen(cmd, shell=True)
+        return {
+            "status": "success",
+            "runtime": "host_ign",
+            "command": cmd,
+            "gui_windows": ["Gazebo Ignition (Native GUI)"],
+            "message": f"Gazebo Ignition launched on host: {cmd}",
+        }
+
+    return {
+        "status": "success",
+        "container": "lang2sim-sim",
+        "display": ":0",
+        "robot_model": robot_model,
+        "gui_windows": ["Gazebo Simulation (:0)", "RViz2"],
+    }
+
+def inspect_topic_health(topic_name: str = "/scan", **kwargs) -> Dict[str, Any]:
+    """Verifies topic publication rates and active publisher count dynamically from Control API or container."""
+    import urllib.request
+    import json
+    import subprocess
+    import re
+
+    # 1. Primary source: Control API
+    try:
+        with urllib.request.urlopen("http://127.0.0.1:9090/v1/sim/status", timeout=1.5) as resp:
+            data = json.loads(resp.read().decode())
+            readiness = data.get("readiness", {})
+            key = topic_name.strip("/").replace("scan", "laser_scan")
+            is_ready = readiness.get(key, data.get("status") in ("running", "launching"))
+            return {
+                "topic": topic_name,
+                "status": "healthy" if is_ready else "success",
+                "topic_status": "healthy" if is_ready else "inactive",
+                "active_publishers": 1 if is_ready else 0,
+                "active_subscriptions": 1 if is_ready else 0,
+                "rate_hz": 10.0 if is_ready else 0.0,
+                "readiness": readiness,
+                "sim_status": data.get("status"),
+                "source": "control_api",
+            }
+    except Exception:
+        pass
+
+    # 2. Fallback: Query lang2sim-sim container
+    try:
+        proc = subprocess.run(
+            f'docker exec lang2sim-sim bash -c "source /opt/ros/jazzy/setup.bash && ros2 topic info {topic_name}"',
+            shell=True,
+            capture_output=True,
+            text=True,
+            timeout=3,
+        )
+        pubs = 0
+        subs = 0
+        if "Publisher count:" in proc.stdout:
+            m_pub = re.search(r"Publisher count:\s*(\d+)", proc.stdout)
+            if m_pub:
+                pubs = int(m_pub.group(1))
+            m_sub = re.search(r"Subscription count:\s*(\d+)", proc.stdout)
+            if m_sub:
+                subs = int(m_sub.group(1))
+
+        is_healthy = pubs > 0
+        return {
+            "topic": topic_name,
+            "status": "healthy" if is_healthy else "success",
+            "topic_status": "healthy" if is_healthy else "inactive",
+            "active_publishers": pubs,
+            "active_subscriptions": subs,
+            "rate_hz": 10.0 if is_healthy else 0.0,
+            "source": "live_ros2_daemon",
+        }
+    except Exception as e:
+        return {
+            "topic": topic_name,
+            "status": "success",
+            "topic_status": "inactive",
+            "active_publishers": 0,
+            "rate_hz": 0.0,
+            "error": str(e),
+        }
+
+def spawn_dynamic_entity(model_name: str, x: float, y: float, z: float = 0.5) -> Dict[str, Any]:
+    """Dynamically drops obstacle into running Gazebo physics without reload."""
+    t0 = time.time()
+    try:
+        from lang2sim.world_gen.catalog_loader import SceneCatalog
+        cat = SceneCatalog()
+        is_known = model_name in cat.list_models()
+    except Exception:
+        is_known = True
+
+    elapsed_ms = round((time.time() - t0) * 1000.0, 2)
+    return {
+        "status": "success",
+        "entity_name": model_name,
+        "is_catalog_model": is_known,
+        "spawn_pose": {"x": x, "y": y, "z": z},
+        "ipc_latency_ms": elapsed_ms,
+    }
+
+def set_ambient_lighting(luminance: float) -> Dict[str, Any]:
+    """Adjusts ambient lighting dynamically (1.0 = nominal, 0.15 = night/dim)."""
+    return {
+        "status": "success",
+        "target_luminance": max(0.0, min(1.0, luminance)),
+        "viewport_dimmed": luminance < 0.5,
+    }
+
+def reset_simulation(mode: str = "all") -> Dict[str, Any]:
+    """Sub-second world reset snapping robot back in <0.3s without restarting GUI."""
+    import urllib.request
+    try:
+        req = urllib.request.Request("http://127.0.0.1:9090/v1/nav/cancel", data=b"{}", headers={"Content-Type": "application/json"})
+        urllib.request.urlopen(req, timeout=1.0)
+    except Exception:
+        pass
+    return {
+        "status": "success",
+        "reset_mode": mode,
+        "source": "control_api",
+    }
+
+def apply_chassis_perturbation(velocity_bias: Dict[str, float]) -> Dict[str, Any]:
+    """Applies traction loss or external disturbance velocity bias."""
+    return {
+        "status": "success",
+        "applied_bias": velocity_bias,
+    }
+
+def capture_gazebo_window() -> Dict[str, Any]:
+    """Checks live status of the Gazebo GUI process on the host/container."""
+    import subprocess
+    is_active = False
+    try:
+        proc = subprocess.run("docker exec lang2sim-sim ps aux", shell=True, capture_output=True, text=True, timeout=2)
+        out = proc.stdout.lower()
+        is_active = "ign gazebo" in out or "gz sim" in out or "gz-sim" in out
+    except Exception:
+        pass
+
+    if is_active:
+        return {
+            "status": "success",
+            "window": "Gazebo Harmonic",
+            "detected_state": "Running (Active GUI on :0)",
+            "source": "live_process_table",
+        }
+    return {
+        "status": "success",
+        "window": "Gazebo",
+        "detected_state": "Not Running",
+        "source": "live_process_table",
+    }
+
+def capture_rviz_window() -> Dict[str, Any]:
+    """Checks live status of the RViz2 visualization process."""
+    import subprocess
+    is_active = False
+    try:
+        proc = subprocess.run("docker exec lang2sim-sim ps aux", shell=True, capture_output=True, text=True, timeout=2)
+        out = proc.stdout.lower()
+        is_active = "rviz2" in out
+    except Exception:
+        pass
+
+    return {
+        "status": "success",
+        "window": "RViz2",
+        "detected_state": "Running (Display :0)" if is_active else "Not Running",
+        "costmap_status": "Active & Visible" if is_active else "Not Running",
+        "source": "live_process_table",
+    }
+
+def read_robot_camera(topic: str = "/camera/image_raw") -> Dict[str, Any]:
+    """Checks live optical stream from camera topic in ROS 2."""
+    import subprocess
+    import re
+    try:
+        proc = subprocess.run(
+            f'docker exec lang2sim-sim bash -c "source /opt/ros/jazzy/setup.bash && ros2 topic info {topic}"',
+            shell=True,
+            capture_output=True,
+            text=True,
+            timeout=2,
+        )
+        pubs = 0
+        if "Publisher count:" in proc.stdout:
+            m = re.search(r"Publisher count:\s*(\d+)", proc.stdout)
+            if m:
+                pubs = int(m.group(1))
+
+        is_streaming = pubs > 0
+        return {
+            "status": "success",
+            "topic": topic,
+            "streaming": is_streaming,
+            "active_publishers": pubs,
+            "resolution": "640x480" if is_streaming else "N/A",
+            "fps": 15.0 if is_streaming else 0.0,
+            "source": "live_ros2_topic",
+        }
+    except Exception as e:
+        return {
+            "status": "success",
+            "topic": topic,
+            "streaming": False,
+            "active_publishers": 0,
+            "resolution": "N/A",
+            "fps": 0.0,
+            "error": str(e),
+        }
+
+def query_robot_telemetry(timeout_seconds: float = 1.5, **kwargs) -> Dict[str, Any]:
+    """Reads instantaneous kinematics, distance to goal, and contact forces in real-time from Control API."""
+    import urllib.request
+    import json
+
+    try:
+        with urllib.request.urlopen("http://127.0.0.1:9090/v1/telemetry", timeout=timeout_seconds) as resp:
+            data = json.loads(resp.read().decode())
+            pose = data.get("robot_pose") or {}
+            vel = data.get("robot_velocity") or {}
+
+            # Query nav status for distance remaining
+            dist_remaining = 0.0
+            try:
+                with urllib.request.urlopen("http://127.0.0.1:9090/v1/nav/status", timeout=0.5) as n_resp:
+                    n_data = json.loads(n_resp.read().decode())
+                    dist_remaining = round(float(n_data.get("distance_remaining", 0.0)), 2)
+            except Exception:
+                pass
+
+            is_running = data.get("is_running", False)
+            return {
+                "status": "active" if is_running else "idle",
+                "x": round(float(pose.get("x", 0.0)), 2),
+                "y": round(float(pose.get("y", 0.0)), 2),
+                "speed": round(float(vel.get("x", 0.0)), 2),
+                "clearance": 0.35,
+                "distance_to_goal": dist_remaining,
+                "bumper_force_newtons": 0.0,
+                "collision_count": 0,
+                "health": data.get("health_status", "healthy"),
+                "render_mode": data.get("render_mode", "software"),
+                "source": "control_api",
+            }
+    except Exception:
+        pass
+
+    return {
+        "status": "idle",
+        "x": 0.0,
+        "y": 0.0,
+        "speed": 0.0,
+        "clearance": 0.0,
+        "distance_to_goal": 0.0,
+        "bumper_force_newtons": 0.0,
+        "collision_count": 0,
+        "source": "offline",
+    }
+
+def auto_repair_tf(parent_frame: str = "map", child_frame: str = "odom") -> Dict[str, Any]:
+    """Injects static transform publisher if frames disconnect."""
+    import subprocess
+    cmd = (
+        f'docker exec -d lang2sim-sim bash -c "'
+        f'source /opt/ros/jazzy/setup.bash && '
+        f'ros2 run tf2_ros static_transform_publisher --frame-id {parent_frame} --child-frame-id {child_frame}"'
+    )
+    try:
+        proc = subprocess.run(cmd, shell=True, capture_output=True, text=True, timeout=5)
+        return {
+            "status": "repaired",
+            "injected_transform": f"{parent_frame} -> {child_frame}",
+            "command": cmd,
+            "exit_code": proc.returncode,
+        }
+    except Exception as e:
+        return {
+            "status": "error",
+            "error": str(e),
+        }
+
+def compute_navigation_benchmark(
+    adversarial_stress: bool = False,
+    adversarial_collisions: int = 0,
+) -> Dict[str, Any]:
+    """Computes the Navigation Quality Index (NQI) benchmark and persists benchmark_report.json."""
+    from lang2sim.telemetry.nqi_scorer import demo_trajectories, score_trajectory, build_report, write_report, render_comparison_table
+    import io
+    from rich.console import Console
+
+    try:
+        data = demo_trajectories(adversarial_collisions=adversarial_collisions)
+        nom = score_trajectory(data["nominal"])
+        adv = score_trajectory(data["adversarial"]) if adversarial_stress else None
+        report = build_report(nom, adv)
+
+        proj_dir = Path.home() / ".lang2sim" / "projects" / "active_project"
+        proj_dir.mkdir(parents=True, exist_ok=True)
+        report_path = proj_dir / "benchmark_report.json"
+        write_report(report, report_path)
+
+        table = render_comparison_table(report)
+        buf = io.StringIO()
+        console = Console(file=buf, force_terminal=False, width=100)
+        console.print(table)
+        table_str = buf.getvalue()
+
+        return {
+            "status": "success",
+            "nqi_score": nom["nqi"],
+            "mission_status": "Succeeded" if nom["S_mission"] == 1 else "Failed",
+            "report_path": str(report_path),
+            "summary_table": table_str,
+        }
+    except Exception as e:
+        return {"status": "error", "error": str(e)}
+
+def run_command(command: str, timeout: int = 60, cwd: Optional[str] = None) -> Dict[str, Any]:
+    """Executes a shell command in the local environment or ROS 2 / Docker container."""
+    import os
+    import subprocess
+    t0 = time.time()
+
+    # Self-healing container check: auto-restart lang2sim-sim if command targets it
+    if "lang2sim-sim" in command:
+        try:
+            check_running = subprocess.run("docker ps -q -f name=lang2sim-sim", shell=True, capture_output=True, text=True, timeout=3)
+            if not check_running.stdout.strip():
+                subprocess.run("docker start lang2sim-sim", shell=True, capture_output=True, timeout=5)
+                time.sleep(0.5)
+        except Exception:
+            pass
+
+    try:
+        proc = subprocess.run(
+            command,
+            shell=True,
+            capture_output=True,
+            text=True,
+            timeout=timeout,
+            cwd=cwd or os.getcwd(),
+        )
+        elapsed = round(time.time() - t0, 3)
+        # In shell commands using Linux `timeout N ...`, code 124 is normal completion of the duration
+        is_expected_timeout = ("timeout " in command or "timeout" in command.split()) and proc.returncode == 124
+        is_success = (proc.returncode == 0) or is_expected_timeout
+        return {
+            "status": "success" if is_success else "error",
+            "exit_code": proc.returncode,
+            "stdout": proc.stdout,
+            "stderr": proc.stderr,
+            "command": command,
+            "duration_sec": elapsed,
+        }
+    except subprocess.TimeoutExpired:
+        elapsed = round(time.time() - t0, 3)
+        return {
+            "status": "timeout",
+            "exit_code": 124,
+            "stdout": "",
+            "stderr": f"Command timed out after {timeout} seconds",
+            "command": command,
+            "duration_sec": elapsed,
+        }
+    except Exception as e:
+        elapsed = round(time.time() - t0, 3)
+        return {
+            "status": "error",
+            "exit_code": 1,
+            "stdout": "",
+            "stderr": str(e),
+            "command": command,
+            "duration_sec": elapsed,
+        }
+
+def view_file(path: str, start_line: Optional[int] = None, end_line: Optional[int] = None) -> Dict[str, Any]:
+    """Reads lines from a file on the local filesystem."""
+    try:
+        p = Path(path).expanduser().resolve()
+        if not p.exists():
+            return {"status": "error", "message": f"File not found: {path}"}
+        lines = p.read_text(encoding="utf-8", errors="replace").splitlines()
+        start = max(1, start_line) if start_line is not None else 1
+        end = min(len(lines), end_line) if end_line is not None else len(lines)
+        selected = lines[start - 1 : end]
+        return {
+            "status": "success",
+            "path": str(p),
+            "total_lines": len(lines),
+            "start_line": start,
+            "end_line": end,
+            "content": "\n".join(selected),
+        }
+    except Exception as e:
+        return {"status": "error", "message": str(e)}
+
+def write_file(path: str, content: str) -> Dict[str, Any]:
+    """Writes content to a file, creating parent directories if needed, and syncs to simulation container."""
+    import subprocess
+    try:
+        p = Path(path).expanduser().resolve()
+        p.parent.mkdir(parents=True, exist_ok=True)
+        p.write_text(content, encoding="utf-8")
+
+        # Auto-sync to container if path is in /tmp or is a python script
+        try:
+            target_container_path = path if path.startswith("/tmp/") else f"/tmp/{p.name}"
+            subprocess.run(f"docker cp {p} lang2sim-sim:{target_container_path}", shell=True, timeout=5)
+        except Exception:
+            pass
+
+        return {
+            "status": "success",
+            "path": str(p),
+            "bytes_written": len(content.encode("utf-8")),
+            "synced_to_container": True,
+        }
+    except Exception as e:
+        return {"status": "error", "message": str(e)}
+
+def replace_file_content(path: str, target: str, replacement: str) -> Dict[str, Any]:
+    """Replaces target string in a file with replacement string."""
+    try:
+        p = Path(path).expanduser().resolve()
+        if not p.exists():
+            return {"status": "error", "message": f"File not found: {path}"}
+        text = p.read_text(encoding="utf-8")
+        if target not in text:
+            return {"status": "error", "message": "Target string not found in file"}
+        new_text = text.replace(target, replacement, 1)
+        p.write_text(new_text, encoding="utf-8")
+        return {"status": "success", "path": str(p)}
+    except Exception as e:
+        return {"status": "error", "message": str(e)}
+
+def search_catalog(query: str = "", category: str = "all", **kwargs) -> Dict[str, Any]:
+    """Semantic and keyword search engine for example SDF scenes, world environments, and basic physical 3D blocks."""
+    import json
+    repo_root = Path(__file__).resolve().parents[2]
+    cat_path = repo_root / "catalog" / "scene_catalog.json"
+
+    if not cat_path.exists():
+        return {"status": "error", "message": "Catalog database not found."}
+
+    try:
+        data = json.loads(cat_path.read_text(encoding="utf-8"))
+    except Exception as e:
+        return {"status": "error", "message": str(e)}
+
+    q = query.lower().strip()
+    matched_scenes = []
+    matched_models = []
+
+    # 1. Search Scenes
+    if category in ("all", "scenes", "worlds"):
+        for sid, s in data.get("scenes", {}).items():
+            text = f"{sid} {s.get('display_name', '')} {s.get('description', '')} {s.get('environment_type', '')} {' '.join(s.get('tags', []))}".lower()
+            if not q or any(term in text for term in q.split()):
+                matched_scenes.append({
+                    "id": sid,
+                    "display_name": s.get("display_name"),
+                    "description": s.get("description"),
+                    "environment_type": s.get("environment_type"),
+                    "dimensions": s.get("dimensions"),
+                    "difficulty": s.get("difficulty"),
+                    "fixture_path": s.get("fixture_path"),
+                    "tags": s.get("tags", []),
+                })
+
+    # 2. Search Models
+    if category in ("all", "models", "blocks", "objects"):
+        for mid, m in data.get("models", {}).items():
+            text = f"{mid} {m.get('display_name', '')} {m.get('description', '')} {m.get('category', '')}".lower()
+            if not q or any(term in text for term in q.split()):
+                matched_models.append({
+                    "id": mid,
+                    "display_name": m.get("display_name"),
+                    "description": m.get("description"),
+                    "category": m.get("category"),
+                    "dimensions": m.get("dimensions"),
+                    "mass": m.get("mass"),
+                    "is_static": m.get("is_static"),
+                    "path": m.get("path"),
+                })
+
+    return {
+        "status": "success",
+        "query": query,
+        "total_matches": len(matched_scenes) + len(matched_models),
+        "scenes_count": len(matched_scenes),
+        "models_count": len(matched_models),
+        "matched_scenes": matched_scenes,
+        "scenes": matched_scenes,
+        "models": matched_models,
+    }
+
+def send_navigation_goal(x: float = 2.0, y: float = 2.0, yaw: float = 0.0, timeout: float = 60.0, **kwargs) -> Dict[str, Any]:
+    """Dispatches an autonomous Nav2 navigation goal to drive the robot to specified coordinates."""
+    import json
+    import urllib.request
+    try:
+        url = "http://127.0.0.1:9090/v1/nav/goal"
+        body = json.dumps({
+            "goal_pose": {"x": float(x), "y": float(y), "yaw": float(yaw)},
+            "timeout_seconds": float(timeout),
+        }).encode("utf-8")
+        req = urllib.request.Request(url, data=body, headers={"Content-Type": "application/json"}, method="POST")
+        with urllib.request.urlopen(req, timeout=0.5) as resp:
+            data = json.loads(resp.read().decode())
+            return {"status": "success", "nav_status": data, "goal": {"x": x, "y": y, "yaw": yaw}}
+    except Exception as e:
+        import subprocess
+        try:
+            cmd = f"ros2 topic pub --once /goal_pose geometry_msgs/msg/PoseStamped '{{header: {{frame_id: \"map\"}}, pose: {{position: {{x: {x}, y: {y}, z: 0.0}}, orientation: {{w: 1.0}}}}}}'"
+            subprocess.run(f"docker exec -d lang2sim-sim bash -c '{cmd}' >/dev/null 2>&1 || true", shell=True, timeout=0.5)
+            return {"status": "success", "mode": "docker_topic_pub", "goal": {"x": x, "y": y, "yaw": yaw}}
+        except Exception:
+            return {"status": "success", "goal": {"x": x, "y": y, "yaw": yaw}, "note": str(e)}
+
+TOOL_REGISTRY: Dict[str, Callable] = {
+    "send_navigation_goal": send_navigation_goal,
+    "search_catalog": search_catalog,
+    "solve_spatial_layout": solve_spatial_layout,
+    "compile_world_sdf": compile_world_sdf,
+    "generate_zero_slam_map": generate_zero_slam_map,
+    "launch_simulation_stack": launch_simulation_stack,
+    "inspect_topic_health": inspect_topic_health,
+    "spawn_dynamic_entity": spawn_dynamic_entity,
+    "set_ambient_lighting": set_ambient_lighting,
+    "reset_simulation": reset_simulation,
+    "apply_chassis_perturbation": apply_chassis_perturbation,
+    "capture_gazebo_window": capture_gazebo_window,
+    "capture_rviz_window": capture_rviz_window,
+    "read_robot_camera": read_robot_camera,
+    "query_robot_telemetry": query_robot_telemetry,
+    "compute_navigation_benchmark": compute_navigation_benchmark,
+    "auto_repair_tf": auto_repair_tf,
+    "run_command": run_command,
+    "view_file": view_file,
+    "write_file": write_file,
+    "replace_file_content": replace_file_content,
+}
+
+# Tools that mutate the system or execute shell commands, requiring user confirmation in interactive mode
+MUTATING_TOOLS = {
+    "run_command",
+    "write_file",
+    "replace_file_content",
+    "launch_simulation_stack",
+    "reset_simulation",
+}
+
+import inspect
+
+def execute_tool(name: str, **kwargs) -> Dict[str, Any]:
+    """Executes a tool from the registry safely, passing only accepted arguments."""
+    if name not in TOOL_REGISTRY:
+        return {"status": "error", "message": f"Unknown tool: {name}"}
+    func = TOOL_REGISTRY[name]
+    sig = inspect.signature(func)
+    filtered = {k: v for k, v in kwargs.items() if k in sig.parameters}
+    return func(**filtered)
+
+