lang2sim 2.0.0__py3-none-any.whl

lang2sim/runtime/health_monitor.py

@@ -0,0 +1,66 @@
+"""
+Health monitor: healthy / degraded / crashed, with the reasons.
+File: lang2sim/runtime/health_monitor.py
+
+Pure function over what the API server already knows (process status, the ROS
+snapshot, readiness), plus a little memory so a slow simulation is only
+reported once RTF has stayed low for a while. Feeds /v1/telemetry's
+health_status, /v1/sim/health and the self-healer.
+"""
+
+from __future__ import annotations
+
+from dataclasses import dataclass, field
+from typing import Dict, List, Literal, Optional
+
+from lang2sim.runtime.ros_bridge import RosSnapshot, readiness
+
+Health = Literal["healthy", "degraded", "crashed"]
+
+LOW_RTF = 0.2           # below this the sim is too slow to be useful
+LOW_RTF_FOR_S = 15.0    # ...for this long before we call it degraded
+STALL_S = 5.0           # clock not advancing this long while the process runs = stalled
+
+
+@dataclass
+class HealthReport:
+    health: Health
+    issues: List[str] = field(default_factory=list)
+    rtf: Optional[float] = None
+    sim_time: float = 0.0
+    readiness: Dict[str, bool] = field(default_factory=dict)
+
+
+class HealthMonitor:
+    def __init__(self, low_rtf: float = LOW_RTF, low_rtf_for_s: float = LOW_RTF_FOR_S) -> None:
+        self.low_rtf, self.low_rtf_for_s = low_rtf, low_rtf_for_s
+        self._low_since: Optional[float] = None
+
+    def reset(self) -> None:
+        self._low_since = None
+
+    def evaluate(self, process_status: str, snap: Optional[RosSnapshot]) -> HealthReport:
+        """process_status: SimProcess.status() value (idle/launching/running/degraded/stopped/error)."""
+        if process_status == "error":
+            return HealthReport("crashed", ["simulation process exited with an error"])
+        if process_status in ("idle", "stopped") or snap is None:
+            self.reset()
+            return HealthReport("healthy")
+
+        checks = readiness(snap)
+        issues: List[str] = []
+        if process_status == "launching":
+            return HealthReport("healthy", [], snap.rtf, snap.sim_time, checks)
+
+        # Running (or degraded): every check should hold.
+        issues += [f"{name} lost" for name, ok in checks.items() if not ok]
+        if snap.clock_wall is not None and snap.now - snap.clock_wall > STALL_S:
+            issues.append(f"simulation clock stalled for {snap.now - snap.clock_wall:.0f} s")
+        if snap.rtf is not None and snap.rtf < self.low_rtf:
+            self._low_since = self._low_since or snap.now
+            if snap.now - self._low_since >= self.low_rtf_for_s:
+                issues.append(f"real-time factor {snap.rtf:.2f} below {self.low_rtf} for "
+                              f"{snap.now - self._low_since:.0f} s")
+        else:
+            self._low_since = None
+        return HealthReport("degraded" if issues else "healthy", issues, snap.rtf, snap.sim_time, checks)

lang2sim/runtime/launch_builder.py

@@ -0,0 +1,137 @@
+"""
+Launch generator: one self-contained sim_launch.py per run.
+File: lang2sim/runtime/launch_builder.py
+
+Placed in runtime/ rather than compiler/ because the container imports it and
+compiler/__init__.py pulls in jinja2/shapely, which the sim image does not ship.
+
+The generated file is plain ROS 2 Python launch code with this run's settings
+baked in as a dict literal at the top, so it can be read, diffed and re-run by
+hand:  ros2 launch <run_dir>/sim_launch.py
+
+It starts: Gazebo server (+ optional GUI that may be closed without stopping the
+sim), robot_state_publisher, the TurtleBot3 Waffle spawned at the manifest's
+pose, ros_gz_bridge (+ optional camera bridge), Nav2 with this run's map and AMCL
+seeded at the spawn pose, and optional RViz2.
+"""
+
+from __future__ import annotations
+
+import pprint
+from dataclasses import asdict, dataclass, field
+from pathlib import Path
+from typing import List
+
+LAUNCH_FILE = "sim_launch.py"
+
+
+@dataclass(frozen=True)
+class LaunchConfig:
+    run_dir: str                       # container path; logs and generated params go here
+    world_sdf: str                     # container path, already checked/normalized
+    world_name: str
+    map_yaml: str
+    x: float = 0.0
+    y: float = 0.0
+    yaw: float = 0.0
+    gui: bool = True
+    rviz: bool = True
+    camera: bool = True                # image bridge; heavy under software rendering
+    nav2_params: str = ""              # "" -> TurtleBot3 Waffle params
+    nav2_delay_s: float = 5.0
+    resource_paths: List[str] = field(default_factory=list)  # extra GZ_SIM_RESOURCE_PATH entries
+    robot: str = "waffle"
+
+
+_TEMPLATE = '''\
+"""Generated by lang2sim (lang2sim/runtime/launch_builder.py). Do not edit; regenerate instead."""
+import math
+import os
+
+import yaml
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import AppendEnvironmentVariable, IncludeLaunchDescription, TimerAction
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch_ros.actions import Node
+
+CONFIG = __CONFIG__
+
+
+def _nav2_params(tb3_nav):
+    """Base Nav2 params with AMCL seeded at the spawn pose, written into the run folder."""
+    base = CONFIG["nav2_params"] or os.path.join(tb3_nav, "param", CONFIG["robot"] + ".yaml")
+    with open(base) as f:
+        params = yaml.safe_load(f)
+    amcl = params.setdefault("amcl", {}).setdefault("ros__parameters", {})
+    amcl["set_initial_pose"] = True
+    amcl["initial_pose"] = {"x": float(CONFIG["x"]), "y": float(CONFIG["y"]), "z": 0.0,
+                            "yaw": float(CONFIG["yaw"])}
+    out = os.path.join(CONFIG["run_dir"], "nav2_params.l2s.yaml")
+    with open(out, "w") as f:
+        yaml.safe_dump(params, f, sort_keys=False)
+    return out
+
+
+def generate_launch_description():
+    tb3_gz = get_package_share_directory("turtlebot3_gazebo")
+    tb3_nav = get_package_share_directory("turtlebot3_navigation2")
+    ros_gz_sim = get_package_share_directory("ros_gz_sim")
+    nav2_bringup = get_package_share_directory("nav2_bringup")
+    model = "turtlebot3_" + CONFIG["robot"]
+
+    with open(os.path.join(tb3_gz, "urdf", model + ".urdf")) as f:
+        robot_description = f.read()
+
+    actions = [AppendEnvironmentVariable("GZ_SIM_RESOURCE_PATH", p)
+               for p in [os.path.join(tb3_gz, "models"), *CONFIG["resource_paths"]]]
+
+    actions.append(IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(os.path.join(ros_gz_sim, "launch", "gz_sim.launch.py")),
+        launch_arguments={"gz_args": "-r -s -v2 " + CONFIG["world_sdf"], "on_exit_shutdown": "true"}.items()))
+    if CONFIG["gui"]:
+        actions.append(IncludeLaunchDescription(
+            PythonLaunchDescriptionSource(os.path.join(ros_gz_sim, "launch", "gz_sim.launch.py")),
+            launch_arguments={"gz_args": "-g -v2", "on_exit_shutdown": "false"}.items()))
+
+    actions.append(Node(
+        package="robot_state_publisher", executable="robot_state_publisher", name="robot_state_publisher",
+        output="screen", parameters=[{"use_sim_time": True, "robot_description": robot_description}]))
+    actions.append(Node(
+        package="ros_gz_sim", executable="create", output="screen",
+        arguments=["-world", CONFIG["world_name"], "-name", CONFIG["robot"],
+                   "-file", os.path.join(tb3_gz, "models", model, "model.sdf"),
+                   "-x", str(CONFIG["x"]), "-y", str(CONFIG["y"]), "-z", "0.01", "-Y", str(CONFIG["yaw"])]))
+    actions.append(Node(
+        package="ros_gz_bridge", executable="parameter_bridge", name="ros_gz_bridge", output="screen",
+        parameters=[{"config_file": os.path.join(tb3_gz, "params", model + "_bridge.yaml"),
+                     "use_sim_time": True}]))
+    if CONFIG["camera"]:
+        actions.append(Node(package="ros_gz_image", executable="image_bridge", name="ros_gz_image_bridge",
+                            arguments=["/camera/image_raw"], output="screen"))
+
+    actions.append(TimerAction(period=float(CONFIG["nav2_delay_s"]), actions=[IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(os.path.join(nav2_bringup, "launch", "bringup_launch.py")),
+        launch_arguments={"map": CONFIG["map_yaml"], "use_sim_time": "true", "autostart": "true",
+                          "params_file": _nav2_params(tb3_nav)}.items())]))
+
+    if CONFIG["rviz"]:
+        actions.append(Node(package="rviz2", executable="rviz2", name="rviz2", output="log",
+                            arguments=["-d", os.path.join(tb3_nav, "rviz", "tb3_navigation2.rviz")],
+                            parameters=[{"use_sim_time": True}]))
+    return LaunchDescription(actions)
+'''
+
+
+def render(cfg: LaunchConfig) -> str:
+    """Python source of the launch file for this config."""
+    if not cfg.world_name:
+        raise ValueError("world_name is required (the robot is spawned into it by name)")
+    literal = pprint.pformat(asdict(cfg), indent=4, sort_dicts=True, width=100)
+    return _TEMPLATE.replace("__CONFIG__", literal)
+
+
+def write(cfg: LaunchConfig, out_dir: Path | None = None) -> Path:
+    path = Path(out_dir or cfg.run_dir) / LAUNCH_FILE
+    path.write_text(render(cfg), encoding="utf-8")
+    return path

lang2sim/runtime/nav2_dispatcher.py

@@ -0,0 +1,215 @@
+"""
+Nav2 dispatcher: initial pose, goals, waypoint missions, cancel, live status.
+File: lang2sim/runtime/nav2_dispatcher.py
+
+Runs inside the container, in the API server. One worker thread owns one
+nav2_simple_commander.BasicNavigator (it spins itself, so it must never be
+touched from another thread); API handlers only enqueue commands and read a
+lock-protected NavStatus.
+
+Waypoint missions are driven leg by leg with goToPose, so every leg reports
+distance remaining, ETA and recoveries. A new goal preempts the current task.
+
+BasicNavigator.waitUntilNav2Active() is deliberately not used: until AMCL
+answers, it republishes its default (0, 0) pose, which would teleport the
+localization estimate. The server only dispatches once readiness says Nav2 is
+active and the robot is localized.
+
+The navigator is injected via `navigator_factory` / `make_pose` so the logic is
+unit-tested on the host without ROS.
+"""
+
+from __future__ import annotations
+
+import math
+import queue
+import threading
+import time
+from dataclasses import dataclass
+from typing import Any, Callable, List, Optional, Sequence
+
+from lang2sim.core.models import NavStatus, Pose2D
+
+POLL_S = 0.1
+
+
+def _ros_navigator() -> Any:
+    from nav2_simple_commander.robot_navigator import BasicNavigator  # type: ignore[import-not-found]
+    from rclpy.parameter import Parameter  # type: ignore[import-not-found]
+
+    nav = BasicNavigator("l2s_nav_dispatcher")
+    nav.set_parameters([Parameter("use_sim_time", value=True)])
+    return nav
+
+
+def _ros_pose(nav: Any, p: Pose2D) -> Any:
+    from geometry_msgs.msg import PoseStamped  # type: ignore[import-not-found]
+
+    ps = PoseStamped()
+    ps.header.frame_id = "map"
+    ps.header.stamp = nav.get_clock().now().to_msg()
+    ps.pose.position.x, ps.pose.position.y = p.x, p.y
+    ps.pose.orientation.z, ps.pose.orientation.w = math.sin(p.yaw / 2), math.cos(p.yaw / 2)
+    return ps
+
+
+def _seconds(duration: Any) -> float:
+    """builtin_interfaces/Duration (or a float in tests) -> seconds."""
+    if duration is None:
+        return 0.0
+    if isinstance(duration, (int, float)):
+        return float(duration)
+    return duration.sec + duration.nanosec * 1e-9
+
+
+@dataclass
+class _Task:
+    kind: str                      # "goal" | "waypoints" | "initial_pose"
+    poses: List[Pose2D]
+    timeout_s: float = 120.0
+
+
+class NavDispatcher:
+    def __init__(self, navigator_factory: Callable[[], Any] = _ros_navigator,
+                 make_pose: Callable[[Any, Pose2D], Any] = _ros_pose) -> None:
+        self._factory = navigator_factory
+        self._make_pose = make_pose
+        self._lock = threading.Lock()
+        self._status = NavStatus()
+        self._queue: "queue.Queue[_Task]" = queue.Queue()
+        self._cancel = threading.Event()
+        self._stop = threading.Event()
+        self._thread: Optional[threading.Thread] = None
+        self._nav: Any = None
+        self._last: Optional[_Task] = None
+
+    # --- API (any thread) ---------------------------------------------------
+    def status(self) -> NavStatus:
+        with self._lock:
+            return self._status.model_copy()
+
+    def set_initial_pose(self, pose: Pose2D) -> None:
+        self._submit(_Task("initial_pose", [pose]))
+
+    def go_to(self, goal: Pose2D, timeout_s: float = 120.0) -> NavStatus:
+        return self._submit(_Task("goal", [goal], timeout_s))
+
+    def follow(self, waypoints: Sequence[Pose2D], timeout_s: float = 180.0) -> NavStatus:
+        if not waypoints:
+            raise ValueError("at least one waypoint is required")
+        return self._submit(_Task("waypoints", list(waypoints), timeout_s))
+
+    def remaining(self) -> List[Pose2D]:
+        """Poses of the current/last task from the leg in progress onwards (for re-planning)."""
+        with self._lock:
+            task, idx = self._last, self._status.waypoint_index
+        return list(task.poses[idx:]) if task else []
+
+    def resume_with(self, first: Pose2D, timeout_s: Optional[float] = None) -> NavStatus:
+        """Replace the current leg's target with `first`, keep the rest of the mission."""
+        rest = self.remaining()[1:]
+        timeout = timeout_s if timeout_s is not None else (self._last.timeout_s if self._last else 120.0)
+        if rest:
+            return self.follow([first, *rest], timeout)
+        return self.go_to(first, timeout)
+
+    def cancel(self) -> NavStatus:
+        self._drain()
+        if self.status().is_active:
+            self._cancel.set()
+        return self.status()
+
+    def shutdown(self) -> None:
+        self._stop.set()
+        self._cancel.set()
+        if self._thread:
+            self._thread.join(timeout=5)
+
+    # --- internals -------------------------------------------------------------
+    def _submit(self, task: _Task) -> NavStatus:
+        self._ensure_worker()
+        if task.kind != "initial_pose":
+            self._drain()                          # newest command wins
+            if self.status().is_active:
+                self._cancel.set()                 # preempt the running task
+            with self._lock:                       # report the new task immediately
+                self._status = NavStatus(is_active=True, status="navigating", current_task=task.kind,
+                                         target_pose=task.poses[0], waypoints_total=len(task.poses))
+        self._queue.put(task)
+        return self.status()
+
+    def _drain(self) -> None:
+        try:
+            while True:
+                self._queue.get_nowait()
+        except queue.Empty:
+            pass
+
+    def _ensure_worker(self) -> None:
+        if self._thread and self._thread.is_alive():
+            return
+        self._thread = threading.Thread(target=self._worker, name="l2s-nav", daemon=True)
+        self._thread.start()
+
+    def _set(self, **kw: Any) -> None:
+        with self._lock:
+            self._status = self._status.model_copy(update=kw)
+
+    def _worker(self) -> None:
+        self._nav = self._factory()
+        while not self._stop.is_set():
+            try:
+                task = self._queue.get(timeout=0.2)
+            except queue.Empty:
+                continue
+            if task.kind == "initial_pose":
+                self._nav.setInitialPose(self._make_pose(self._nav, task.poses[0]))
+                continue
+            self._cancel.clear()
+            self._last = task
+            self._run(task)
+
+    def _run(self, task: _Task) -> None:
+        total = len(task.poses)
+        for i, pose in enumerate(task.poses):
+            self._set(is_active=True, status="navigating", current_task=task.kind, target_pose=pose,
+                      waypoint_index=i, waypoints_total=total, distance_remaining=0.0,
+                      navigation_time=0.0, eta_seconds=None, recoveries=0, message=None)
+            outcome, msg = self._leg(pose, task.timeout_s)
+            if outcome != "succeeded":
+                if outcome == "canceled" and not self._queue.empty():
+                    return  # preempted: the next task's status is already showing
+                self._set(is_active=False, status=outcome, message=msg)
+                return
+        self._set(is_active=False, status="succeeded", distance_remaining=0.0, eta_seconds=0.0,
+                  message=None)
+
+    def _leg(self, pose: Pose2D, timeout_s: float):
+        nav = self._nav
+        if not nav.goToPose(self._make_pose(nav, pose)):
+            return "failed", "goal rejected by Nav2 (bt_navigator not active?)"
+        timed_out = False
+        while not nav.isTaskComplete():
+            fb = nav.getFeedback()
+            if fb is not None:
+                t = _seconds(getattr(fb, "navigation_time", None))
+                self._set(distance_remaining=round(float(getattr(fb, "distance_remaining", 0.0)), 3),
+                          navigation_time=round(t, 2),
+                          eta_seconds=round(_seconds(getattr(fb, "estimated_time_remaining", None)), 1),
+                          recoveries=int(getattr(fb, "number_of_recoveries", 0)))
+                if t > timeout_s and not timed_out:
+                    timed_out = True
+                    nav.cancelTask()
+            if self._cancel.is_set():
+                nav.cancelTask()
+                self._cancel.clear()
+                return "canceled", "canceled by request"
+            time.sleep(POLL_S)
+        name = getattr(nav.getResult(), "name", str(nav.getResult())).upper()
+        if timed_out:
+            return "failed", f"timed out after {timeout_s:.0f} s (sim time)"
+        if "SUCCEEDED" in name:
+            return "succeeded", None
+        if "CANCELED" in name:
+            return "canceled", "canceled by Nav2"
+        return "failed", f"Nav2 result {name}"

lang2sim/runtime/platform.py

@@ -0,0 +1,137 @@
+"""
+Host platform + Docker detection.
+File: lang2sim/runtime/platform.py
+
+Answers: which OS family are we on (linux / wsl / macos / windows), is there a
+display, and can we reach a Docker daemon (and does it have the NVIDIA runtime).
+"""
+
+from __future__ import annotations
+
+import json
+import os
+import platform as _platform
+import shutil
+import subprocess
+from dataclasses import dataclass, field
+from pathlib import Path
+from typing import Dict, List, Literal, Optional
+
+HostOS = Literal["linux", "wsl", "macos", "windows"]
+Arch = Literal["amd64", "arm64", "unknown"]
+
+_ARCH_ALIASES = {"x86_64": "amd64", "amd64": "amd64", "aarch64": "arm64", "arm64": "arm64"}
+
+
+@dataclass(frozen=True)
+class DockerInfo:
+    cli_path: Optional[str] = None
+    server_reachable: bool = False
+    server_version: Optional[str] = None
+    operating_system: Optional[str] = None  # "Docker Desktop", "Ubuntu 24.04 LTS", ...
+    arch: Arch = "unknown"
+    runtimes: List[str] = field(default_factory=list)
+    mem_total_mb: Optional[int] = None   # memory the daemon's VM/host gives containers
+    cpus: Optional[int] = None
+    error: Optional[str] = None
+
+    @property
+    def installed(self) -> bool:
+        return self.cli_path is not None
+
+    @property
+    def is_desktop(self) -> bool:
+        return bool(self.operating_system and "Docker Desktop" in self.operating_system)
+
+    @property
+    def has_nvidia_runtime(self) -> bool:
+        return "nvidia" in self.runtimes
+
+
+@dataclass(frozen=True)
+class HostPlatform:
+    os: HostOS
+    arch: Arch
+    env: Dict[str, str] = field(default_factory=dict)  # DISPLAY / WAYLAND_DISPLAY / XAUTHORITY
+    has_wslg: bool = False
+    has_x11_socket_dir: bool = False
+    docker: DockerInfo = field(default_factory=DockerInfo)
+
+    @property
+    def display(self) -> Optional[str]:
+        return self.env.get("DISPLAY") or None
+
+
+# ---------------------------------------------------------------------------
+# Pure helpers
+# ---------------------------------------------------------------------------
+
+def normalize_arch(machine: str) -> Arch:
+    return _ARCH_ALIASES.get(machine.lower(), "unknown")  # type: ignore[return-value]
+
+
+def classify_os(system: str, release: str, wsl_interop: bool = False) -> HostOS:
+    s = system.lower()
+    if s == "darwin":
+        return "macos"
+    if s == "windows":
+        return "windows"
+    if s == "linux" and (wsl_interop or "microsoft" in release.lower()):
+        return "wsl"
+    return "linux"
+
+
+def parse_docker_info(info_json: str, cli_path: Optional[str] = None) -> DockerInfo:
+    """Parse `docker info --format '{{json .}}'`."""
+    try:
+        d = json.loads(info_json)
+    except json.JSONDecodeError as e:
+        return DockerInfo(cli_path=cli_path, error=f"bad docker info output: {e}")
+    if d.get("ServerErrors"):
+        return DockerInfo(cli_path=cli_path, error="; ".join(d["ServerErrors"]))
+    return DockerInfo(
+        cli_path=cli_path,
+        server_reachable=True,
+        server_version=d.get("ServerVersion"),
+        operating_system=d.get("OperatingSystem"),
+        arch=normalize_arch(d.get("Architecture", "")),
+        runtimes=sorted((d.get("Runtimes") or {}).keys()),
+        mem_total_mb=int(d["MemTotal"]) // (1024 * 1024) if d.get("MemTotal") else None,
+        cpus=d.get("NCPU"),
+    )
+
+
+# ---------------------------------------------------------------------------
+# Probes
+# ---------------------------------------------------------------------------
+
+def detect_docker(docker: str = "docker", timeout: float = 10.0) -> DockerInfo:
+    cli = shutil.which(docker)
+    if not cli:
+        return DockerInfo(error="docker CLI not found on PATH")
+    try:
+        out = subprocess.run([cli, "info", "--format", "{{json .}}"],
+                             capture_output=True, text=True, timeout=timeout, check=False)
+    except subprocess.TimeoutExpired:
+        return DockerInfo(cli_path=cli, error="`docker info` timed out (daemon starting?)")
+    except OSError as e:
+        return DockerInfo(cli_path=cli, error=str(e))
+    if out.returncode != 0 or not out.stdout.strip().startswith("{"):
+        msg = (out.stderr or out.stdout).strip().splitlines()
+        return DockerInfo(cli_path=cli, error=msg[-1] if msg else "docker daemon not reachable")
+    return parse_docker_info(out.stdout, cli)
+
+
+def detect(docker: str = "docker", probe_docker: bool = True) -> HostPlatform:
+    host_os = classify_os(_platform.system(), _platform.release(),
+                          wsl_interop=os.path.exists("/proc/sys/fs/binfmt_misc/WSLInterop"))
+    env = {k: os.environ[k] for k in ("DISPLAY", "WAYLAND_DISPLAY", "XAUTHORITY", "XDG_RUNTIME_DIR")
+           if os.environ.get(k)}
+    return HostPlatform(
+        os=host_os,
+        arch=normalize_arch(_platform.machine()),
+        env=env,
+        has_wslg=Path("/mnt/wslg").is_dir(),
+        has_x11_socket_dir=Path("/tmp/.X11-unix").is_dir(),
+        docker=detect_docker(docker) if probe_docker else DockerInfo(),
+    )