lang2sim 2.0.0__py3-none-any.whl

lang2sim/runtime/ros_bridge.py

@@ -0,0 +1,183 @@
+"""
+The control API's window into ROS: readiness, telemetry, RTF.
+File: lang2sim/runtime/ros_bridge.py
+
+Runs inside the container, in the API server process, as one rclpy node on its
+own executor thread. It only listens (/clock, /odom, /scan, TF, Nav2 lifecycle
+states); commands go through nav2_dispatcher.
+
+Pure helpers (yaw_from_quaternion, rtf_from_samples, readiness) carry the logic
+and are unit-tested on the host; the RosBridge class needs rclpy and is created
+with RosBridge.try_create(), which returns None where ROS is not installed.
+"""
+
+from __future__ import annotations
+
+import collections
+import math
+import threading
+import time
+from dataclasses import dataclass, field
+from typing import Deque, Dict, Optional, Sequence, Tuple
+
+FRESH_S = 3.0          # a topic counts as alive if a message arrived this recently (wall time)
+RTF_WINDOW_S = 5.0
+NAV2_NODES = ("amcl", "bt_navigator", "controller_server", "planner_server")
+
+
+def yaw_from_quaternion(x: float, y: float, z: float, w: float) -> float:
+    return math.atan2(2.0 * (w * z + x * y), 1.0 - 2.0 * (y * y + z * z))
+
+
+def rtf_from_samples(samples: Sequence[Tuple[float, float]]) -> Optional[float]:
+    """(wall_s, sim_s) pairs, oldest first -> real-time factor over the span, or None."""
+    if len(samples) < 2:
+        return None
+    (w0, s0), (w1, s1) = samples[0], samples[-1]
+    if w1 - w0 <= 0.2:
+        return None
+    return max(0.0, (s1 - s0) / (w1 - w0))
+
+
+@dataclass
+class RosSnapshot:
+    now: float = 0.0                      # wall time of the snapshot
+    clock_wall: Optional[float] = None    # wall time of the last /clock
+    sim_time: float = 0.0
+    odom_wall: Optional[float] = None
+    scan_wall: Optional[float] = None
+    odom_pose: Optional[Tuple[float, float, float]] = None   # x, y, yaw in odom
+    map_pose: Optional[Tuple[float, float, float]] = None    # x, y, yaw in map (TF)
+    velocity: Optional[Tuple[float, float, float]] = None    # vx, vy, wz
+    lifecycle: Dict[str, str] = field(default_factory=dict)  # node -> "active", ...
+    rtf: Optional[float] = None
+
+    def fresh(self, t: Optional[float]) -> bool:
+        return t is not None and self.now - t <= FRESH_S
+
+
+def readiness(s: RosSnapshot) -> Dict[str, bool]:
+    """What 'the simulation is ready to navigate' means, check by check."""
+    return {
+        "gazebo_clock": s.fresh(s.clock_wall),
+        "odometry": s.fresh(s.odom_wall),
+        "laser_scan": s.fresh(s.scan_wall),
+        "nav2_active": all(s.lifecycle.get(n) == "active" for n in NAV2_NODES),
+        "localized": s.map_pose is not None,
+    }
+
+
+class RosBridge:
+    """rclpy node + executor thread. Create with try_create()."""
+
+    def __init__(self) -> None:
+        import rclpy  # type: ignore[import-not-found]
+        from lifecycle_msgs.srv import GetState  # type: ignore[import-not-found]
+        from nav_msgs.msg import Odometry  # type: ignore[import-not-found]
+        from rclpy.executors import MultiThreadedExecutor  # type: ignore[import-not-found]
+        from rclpy.node import Node  # type: ignore[import-not-found]
+        from rclpy.parameter import Parameter  # type: ignore[import-not-found]
+        from rclpy.qos import qos_profile_sensor_data  # type: ignore[import-not-found]
+        from rosgraph_msgs.msg import Clock  # type: ignore[import-not-found]
+        from sensor_msgs.msg import LaserScan  # type: ignore[import-not-found]
+        from tf2_ros import Buffer, TransformListener  # type: ignore[import-not-found]
+
+        if not rclpy.ok():
+            rclpy.init()
+        self._rclpy = rclpy
+        self._lock = threading.Lock()
+        self._s = RosSnapshot()
+        self._clock_samples: Deque[Tuple[float, float]] = collections.deque(maxlen=400)
+
+        node = Node("l2s_control_api", parameter_overrides=[Parameter("use_sim_time", value=True)])
+        self.node = node
+        node.create_subscription(Clock, "/clock", self._on_clock, qos_profile_sensor_data)
+        node.create_subscription(Odometry, "/odom", self._on_odom, qos_profile_sensor_data)
+        node.create_subscription(LaserScan, "/scan", self._on_scan, qos_profile_sensor_data)
+        self._tf = Buffer()
+        self._tf_listener = TransformListener(self._tf, node, spin_thread=False)
+        self._GetState = GetState
+        self._lc_clients = {n: node.create_client(GetState, f"/{n}/get_state") for n in NAV2_NODES}
+        node.create_timer(1.0, self._poll)
+
+        self._executor = MultiThreadedExecutor(num_threads=2)
+        self._executor.add_node(node)
+        self._thread = threading.Thread(target=self._executor.spin, name="l2s-ros", daemon=True)
+        self._thread.start()
+
+    @classmethod
+    def try_create(cls) -> Optional["RosBridge"]:
+        try:
+            return cls()
+        except Exception:  # noqa: BLE001 - no ROS here (host tests) or rclpy failed to init
+            return None
+
+    # --- callbacks (executor threads) -------------------------------------
+    def _on_clock(self, msg) -> None:
+        now = time.time()
+        sim = msg.clock.sec + msg.clock.nanosec * 1e-9
+        with self._lock:
+            self._s.clock_wall, self._s.sim_time = now, sim
+            self._clock_samples.append((now, sim))
+            while self._clock_samples and now - self._clock_samples[0][0] > RTF_WINDOW_S:
+                self._clock_samples.popleft()
+
+    def _on_odom(self, msg) -> None:
+        p, q, t = msg.pose.pose.position, msg.pose.pose.orientation, msg.twist.twist
+        with self._lock:
+            self._s.odom_wall = time.time()
+            self._s.odom_pose = (p.x, p.y, yaw_from_quaternion(q.x, q.y, q.z, q.w))
+            self._s.velocity = (t.linear.x, t.linear.y, t.angular.z)
+
+    def _on_scan(self, _msg) -> None:
+        with self._lock:
+            self._s.scan_wall = time.time()
+
+    def _poll(self) -> None:
+        # Map pose from TF (AMCL map->odom + odom->base_footprint)
+        try:
+            from rclpy.time import Time
+            tr = self._tf.lookup_transform("map", "base_footprint", Time())
+            tt, q = tr.transform.translation, tr.transform.rotation
+            pose = (tt.x, tt.y, yaw_from_quaternion(q.x, q.y, q.z, q.w))
+        except Exception:  # noqa: BLE001 - not localized yet
+            pose = None
+        with self._lock:
+            self._s.map_pose = pose
+        # Nav2 lifecycle states, asynchronously
+        for name, client in self._lc_clients.items():
+            if not client.service_is_ready():
+                with self._lock:
+                    self._s.lifecycle.pop(name, None)
+                continue
+            fut = client.call_async(self._GetState.Request())
+            fut.add_done_callback(lambda f, n=name: self._on_state(n, f))
+
+    def _on_state(self, name: str, fut) -> None:
+        try:
+            label = fut.result().current_state.label
+        except Exception:  # noqa: BLE001
+            return
+        with self._lock:
+            self._s.lifecycle[name] = label
+
+    # --- API ---------------------------------------------------------------
+    def snapshot(self) -> RosSnapshot:
+        with self._lock:
+            s = RosSnapshot(**{k: (dict(v) if isinstance(v, dict) else v) for k, v in vars(self._s).items()})
+            s.rtf = rtf_from_samples(list(self._clock_samples))
+        s.now = time.time()
+        return s
+
+    def reset(self) -> None:
+        """Forget state from a previous run (called on /sim/start)."""
+        with self._lock:
+            self._s = RosSnapshot()
+            self._clock_samples.clear()
+
+    def stop(self) -> None:
+        try:
+            self._executor.shutdown(timeout_sec=2)
+            self.node.destroy_node()
+        except Exception:  # noqa: BLE001
+            pass

lang2sim/runtime/run_folder.py

@@ -0,0 +1,86 @@
+"""
+Run folder: the one input Part 3 accepts (decision 1).
+File: lang2sim/runtime/run_folder.py
+
+    ~/.lang2sim/projects/<project>/runs/<timestamp>/
+        world.sdf        Gazebo Harmonic world (Part 1)
+        map.yaml/.pgm    Nav2 map, map frame == world frame (Part 1)
+        spec.json        the SimSpec (Part 2 / Part 1)
+        manifest.json    RunManifest: file names, world name, robot, initial pose
+        logs/            written by Part 3 (sim.log, heal_log.jsonl)
+
+Host side only: load the manifest (or infer it from spec.json), check the files
+exist, and turn it into the SimStartRequest the container API expects.
+"""
+
+from __future__ import annotations
+
+import importlib
+import json
+from pathlib import Path
+from typing import Optional
+
+from lang2sim.core.models import Pose2D, RunManifest, SimSpec, SimStartRequest
+from lang2sim.runtime.docker_runner import host_to_container_path
+
+cfg = importlib.import_module("lang2sim.core.config")
+
+MANIFEST = "manifest.json"
+
+
+class RunFolderError(ValueError):
+    """The folder does not satisfy the run-folder contract."""
+
+
+def load_manifest(run_dir: Path) -> RunManifest:
+    """manifest.json if present, else inferred from spec.json (initial pose from its mission)."""
+    run_dir = Path(run_dir)
+    if not run_dir.is_dir():
+        raise RunFolderError(f"run folder not found: {run_dir}")
+    mpath = run_dir / MANIFEST
+    if mpath.is_file():
+        try:
+            manifest = RunManifest.model_validate_json(mpath.read_text())
+        except ValueError as e:
+            raise RunFolderError(f"{mpath} is invalid: {e}") from e
+    else:
+        spec_path = run_dir / "spec.json"
+        if not spec_path.is_file():
+            raise RunFolderError(f"{run_dir} has neither {MANIFEST} nor spec.json")
+        spec = SimSpec.model_validate_json(spec_path.read_text())
+        manifest = RunManifest(initial_pose=spec.mission.initial_pose)
+
+    missing = [n for n in (manifest.world_sdf, manifest.map_yaml) if not (run_dir / n).is_file()]
+    if missing:
+        raise RunFolderError(f"{run_dir} is missing {', '.join(missing)}")
+    return manifest
+
+
+def write_manifest(run_dir: Path, manifest: RunManifest) -> Path:
+    path = Path(run_dir) / MANIFEST
+    path.write_text(json.dumps(manifest.model_dump(mode="json"), indent=2) + "\n")
+    return path
+
+
+def to_start_request(run_dir: Path, manifest: Optional[RunManifest] = None, *,
+                     projects_dir: Optional[Path] = None, gui: bool = True, rviz: bool = True,
+                     initial_pose: Optional[Pose2D] = None) -> SimStartRequest:
+    """Container-side SimStartRequest for a host run folder under projects_dir."""
+    run_dir = Path(run_dir)
+    manifest = manifest or load_manifest(run_dir)
+    projects = Path(projects_dir) if projects_dir else cfg.get_projects_dir()
+    try:
+        c_run = host_to_container_path(run_dir, projects)
+    except ValueError as e:
+        raise RunFolderError(f"{run_dir} must be inside the projects folder {projects}") from e
+    return SimStartRequest(
+        run_dir=c_run,
+        world_sdf_path=f"{c_run}/{manifest.world_sdf}",
+        map_yaml_path=f"{c_run}/{manifest.map_yaml}",
+        robot_model=manifest.robot_model,
+        initial_pose=initial_pose or manifest.initial_pose,
+        use_gui=gui,
+        use_rviz=rviz,
+        world_name=manifest.world_name,
+        nav2_params_path=f"{c_run}/{manifest.nav2_params}" if manifest.nav2_params else None,
+    )

lang2sim/runtime/self_healing.py

@@ -0,0 +1,387 @@
+"""
+Self-healing supervisor: classify -> plan_recovery -> rate-limited executor.
+File: lang2sim/runtime/self_healing.py
+
+Runs inside the container, in the API server, every few seconds while a sim is
+up. Inputs are what the server already has: new sim log lines, the ROS
+snapshot/readiness, the nav status and which node processes are alive.
+
+Four rules (issue #3, plan v2):
+
+| Fault            | Seen as                                                  | Recovery                          |
+|------------------|----------------------------------------------------------|-----------------------------------|
+| missing_package  | "Package 'x' not found" / ModuleNotFoundError in the log | report only: never install at runtime |
+| missing_tf       | robot_state_publisher dead (process gone, "process has   | restart robot_state_publisher     |
+|                  | died", or TF errors while it is not running)             | (TF errors alone: reported once)  |
+| unlinked_scan    | ros_gz_bridge dead, or no /scan while the clock runs     | restart the bridge                |
+| goal_in_collision| planner says the goal is occupied / no valid path        | move the goal to the nearest free |
+|                  |                                                          | map cell and resend it            |
+
+Each action is rate-limited per fault (cooldown + max attempts in a window) and
+written to <run>/logs/heal_log.jsonl as a HealEvent plus details.
+"""
+
+from __future__ import annotations
+
+import json
+import math
+import os
+import re
+import subprocess
+import threading
+import time
+from dataclasses import dataclass, field
+from pathlib import Path
+from typing import Callable, Dict, Iterable, List, Optional, Tuple
+
+from lang2sim.core.models import HealEvent, NavStatus, Pose2D
+from lang2sim.runtime.ros_bridge import RosSnapshot
+
+# ---------------------------------------------------------------------------
+# Faults
+# ---------------------------------------------------------------------------
+
+PKG_PATTERNS = (
+    re.compile(r"[Pp]ackage '([\w\-]+)' not found"),
+    re.compile(r"PackageNotFoundError: .*?\"?'?([\w\-]+)'?\"?$"),
+    re.compile(r"No module named '([\w\.]+)'"),
+)
+TF_PATTERNS = (
+    re.compile(r"Could not find a connection between '(\w+)' and '(\w+)'"),
+    re.compile(r"Timed out waiting for transform from (\w+) to (\w+)"),
+    re.compile(r'"(\w+)" passed to lookupTransform argument \w+ does not exist'),
+)
+GOAL_PATTERNS = (
+    re.compile(r"[Gg]oal (?:coordinates|pose)?.*(?:occupied|in collision|lethal)"),
+    re.compile(r"(?:GridBased|NavFn|Smac).*[Ff]ailed to create (?:a )?plan"),
+    re.compile(r"[Ff]ailed to create plan with tolerance"),
+)
+DIED = re.compile(r"\[(robot_state_publisher|parameter_bridge|ros_gz_bridge)[\w\-]*\]: process has died")
+
+# healer name -> (ROS package, executable); processes are matched on "lib/<pkg>/<exe>"
+NODES = {"robot_state_publisher": ("robot_state_publisher", "robot_state_publisher"),
+         "bridge": ("ros_gz_bridge", "parameter_bridge")}
+SCAN_GRACE_S = 5.0
+TF_GRACE_S = 10.0
+
+
+@dataclass(frozen=True)
+class Fault:
+    kind: str          # missing_package | missing_tf | unlinked_scan | goal_in_collision
+    signature: str     # stable text used for rate limiting and the log
+    evidence: str
+
+
+@dataclass
+class Observation:
+    lines: List[str]                            # new log lines since the last check
+    snap: Optional[RosSnapshot]
+    alive: Dict[str, bool]                      # node process name -> running
+    nav: Optional[NavStatus] = None
+    sim_running: bool = True
+    was_localized: bool = False                 # robot was localized earlier in this run
+
+
+def classify(obs: Observation) -> List[Fault]:
+    faults: Dict[str, Fault] = {}
+
+    def add(kind: str, sig: str, ev: str) -> None:
+        faults.setdefault(f"{kind}:{sig}", Fault(kind, sig, ev))
+
+    for line in obs.lines:
+        for p in PKG_PATTERNS:
+            m = p.search(line)
+            if m:
+                add("missing_package", m.group(1), line.strip()[:240])
+        m = DIED.search(line)
+        if m:
+            kind = "missing_tf" if m.group(1) == "robot_state_publisher" else "unlinked_scan"
+            add(kind, f"{m.group(1)} died", line.strip()[:240])
+        if obs.nav is not None and (obs.nav.is_active or obs.nav.status == "failed"):
+            for p in GOAL_PATTERNS:
+                if p.search(line):
+                    add("goal_in_collision", "goal not plannable", line.strip()[:240])
+        for p in TF_PATTERNS:
+            if p.search(line) and obs.was_localized:
+                # Nav2 keeps logging these for a while after robot_state_publisher is
+                # restarted; only a dead publisher justifies restarting it. Otherwise
+                # report once, never kill a healthy node over stale warnings.
+                if obs.alive.get("robot_state_publisher") is False:
+                    add("missing_tf", "tf lookup failing", line.strip()[:240])
+                else:
+                    add("tf_warning", "tf lookups failing", line.strip()[:240])
+
+    if obs.sim_running:
+        if obs.alive.get("robot_state_publisher") is False:
+            add("missing_tf", "robot_state_publisher not running", "process not found")
+        if obs.alive.get("bridge") is False:
+            add("unlinked_scan", "ros_gz_bridge not running", "process not found")
+        s = obs.snap
+        if s is not None and s.fresh(s.clock_wall):
+            if s.scan_wall is None or s.now - s.scan_wall > SCAN_GRACE_S:
+                if obs.was_localized:  # scan worked before in this run
+                    add("unlinked_scan", "no /scan while the clock runs",
+                        f"last scan {'never' if s.scan_wall is None else f'{s.now - s.scan_wall:.0f} s ago'}")
+    return list(faults.values())
+
+
+# ---------------------------------------------------------------------------
+# Recovery plans
+# ---------------------------------------------------------------------------
+
+@dataclass(frozen=True)
+class Action:
+    kind: str                       # report | restart_node | snap_goal
+    target: str = ""
+    detail: Dict[str, object] = field(default_factory=dict)
+
+
+def plan_recovery(f: Fault) -> Action:
+    if f.kind == "missing_package":
+        return Action("report", f.signature, {"hint": "the sim image is missing this; rebuild it - "
+                                                       "nothing is installed at runtime"})
+    if f.kind == "missing_tf":
+        return Action("restart_node", "robot_state_publisher")
+    if f.kind == "unlinked_scan":
+        return Action("restart_node", "bridge")
+    if f.kind == "goal_in_collision":
+        return Action("snap_goal")
+    if f.kind == "tf_warning":
+        return Action("report", f.signature, {"hint": "TF lookups failed while robot_state_publisher "
+                                                       "was running; no restart (often transient)"})
+    return Action("report", f.signature)
+
+
+# ---------------------------------------------------------------------------
+# Map helpers (goal snapping)
+# ---------------------------------------------------------------------------
+
+def read_map(map_yaml: Path) -> Tuple[List[bytes], int, int, float, Tuple[float, float], float]:
+    """Parse a Nav2 map.yaml + binary PGM. Returns rows (top first), w, h, resolution, origin, free_thresh."""
+    meta: Dict[str, str] = {}
+    for line in Path(map_yaml).read_text().splitlines():
+        if ":" in line:
+            k, v = line.split(":", 1)
+            meta[k.strip()] = v.strip()
+    res = float(meta["resolution"])
+    ox, oy = [float(x) for x in meta["origin"].strip("[]").split(",")[:2]]
+    negate = meta.get("negate", "0").strip() == "1"
+    free_thresh = float(meta.get("free_thresh", "0.196"))
+    data = (Path(map_yaml).parent / meta["image"]).read_bytes()
+    tokens, i = [], 0
+    while len(tokens) < 4:                       # P5 header: magic, w, h, maxval (skip comments)
+        while data[i:i + 1].isspace():
+            i += 1
+        if data[i:i + 1] == b"#":
+            while data[i:i + 1] not in (b"\n", b""):
+                i += 1
+            continue
+        j = i
+        while not data[j:j + 1].isspace():
+            j += 1
+        tokens.append(data[i:j])
+        i = j
+    if tokens[0] != b"P5":
+        raise ValueError("only binary PGM (P5) maps are supported")
+    w, h = int(tokens[1]), int(tokens[2])
+    pix = data[i + 1:i + 1 + w * h]
+    if negate:
+        pix = bytes(255 - b for b in pix)
+    rows = [pix[r * w:(r + 1) * w] for r in range(h)]
+    return rows, w, h, res, (ox, oy), free_thresh
+
+
+def nearest_free(map_yaml: Path, x: float, y: float, clearance: float = 0.45,
+                 max_radius: float = 3.0) -> Optional[Pose2D]:
+    """Closest map point to (x, y) with every cell within `clearance` free. None if none nearby."""
+    rows, w, h, res, (ox, oy), free_thresh = read_map(map_yaml)
+    free_min = int((1 - free_thresh) * 255)      # brighter than this = free
+
+    def free(c: int, r: int) -> bool:
+        return 0 <= c < w and 0 <= r < h and rows[r][c] >= free_min
+
+    k = int(math.ceil(clearance / res))
+    disk = [(dc, dr) for dc in range(-k, k + 1) for dr in range(-k, k + 1) if dc * dc + dr * dr <= k * k]
+
+    def clear(c: int, r: int) -> bool:
+        return all(free(c + dc, r + dr) for dc, dr in disk)
+
+    c0, r0 = int((x - ox) / res), h - 1 - int((y - oy) / res)
+    for ring in range(0, int(max_radius / res) + 1):
+        best = None
+        for dc in range(-ring, ring + 1):
+            for dr in (-ring, ring) if abs(dc) != ring else range(-ring, ring + 1):
+                c, r = c0 + dc, r0 + dr
+                if clear(c, r):
+                    d = dc * dc + dr * dr
+                    if best is None or d < best[0]:
+                        best = (d, c, r)
+        if best:
+            _, c, r = best
+            return Pose2D(x=round(ox + (c + 0.5) * res, 3), y=round(oy + (h - 1 - r + 0.5) * res, 3))
+    return None
+
+
+# ---------------------------------------------------------------------------
+# Executor
+# ---------------------------------------------------------------------------
+
+@dataclass
+class RateLimit:
+    cooldown_s: float = 15.0
+    max_attempts: int = 3
+    window_s: float = 300.0
+
+
+class SelfHealer:
+    """Plans and executes recoveries. Side effects are injected so tests can fake them."""
+
+    def __init__(self, run_dir: Path, *, restart_node: Callable[[str], bool],
+                 resend_goal: Optional[Callable[[Pose2D], bool]] = None,
+                 current_goal: Optional[Callable[[], Optional[Pose2D]]] = None,
+                 map_yaml: Optional[Path] = None, limit: Optional[RateLimit] = None,
+                 clock: Callable[[], float] = time.time) -> None:
+        self.run_dir = Path(run_dir)
+        self.log_path = self.run_dir / "logs" / "heal_log.jsonl"
+        self._restart = restart_node
+        self._resend = resend_goal
+        self._goal = current_goal
+        self._map = map_yaml
+        self.limit = limit or RateLimit()
+        self._clock = clock
+        self._history: Dict[str, List[float]] = {}
+        self._reported: set = set()
+        self.events: List[dict] = []
+
+    def _allowed(self, sig: str) -> Tuple[bool, str]:
+        now = self._clock()
+        hist = [t for t in self._history.get(sig, []) if now - t <= self.limit.window_s]
+        self._history[sig] = hist
+        if hist and now - hist[-1] < self.limit.cooldown_s:
+            return False, "cooldown"
+        if len(hist) >= self.limit.max_attempts:
+            return False, "attempt limit reached"
+        return True, ""
+
+    def handle(self, faults: Iterable[Fault]) -> List[dict]:
+        """Plan and run recoveries. Rate limits are per recovery action (e.g. 'restart_node bridge'),
+        so several symptoms of one failure trigger one restart, at most once per pass."""
+        out: List[dict] = []
+        done_this_pass: set = set()
+        for f in faults:
+            action = plan_recovery(f)
+            if action.kind == "report":
+                key = f"{f.kind}:{f.signature}"
+                if key in self._reported:
+                    continue
+                self._reported.add(key)
+                out.append(self._log(f, action, success=False, note=str(action.detail.get("hint", ""))))
+                continue
+            key = f"{action.kind}:{action.target}"
+            if key in done_this_pass:
+                continue
+            done_this_pass.add(key)
+            ok, why = self._allowed(key)
+            if not ok:
+                if why == "attempt limit reached" and f"{key}:gaveup" not in self._reported:
+                    self._reported.add(f"{key}:gaveup")
+                    out.append(self._log(f, Action("give_up", action.target), success=False,
+                                         note=f"{self.limit.max_attempts} attempts in "
+                                              f"{self.limit.window_s:.0f} s; needs a person"))
+                continue
+            self._history.setdefault(key, []).append(self._clock())
+            success, note = self._execute(action)
+            out.append(self._log(f, action, success, note))
+        return out
+
+    def _execute(self, a: Action) -> Tuple[bool, str]:
+        try:
+            if a.kind == "restart_node":
+                return bool(self._restart(a.target)), f"restarted {a.target}"
+            if a.kind == "snap_goal":
+                goal = self._goal() if self._goal else None
+                if goal is None or self._map is None or self._resend is None:
+                    return False, "no active goal or map to snap against"
+                free = nearest_free(self._map, goal.x, goal.y)
+                if free is None:
+                    return False, f"no free cell within 3 m of ({goal.x}, {goal.y})"
+                free = free.model_copy(update={"yaw": goal.yaw})
+                return bool(self._resend(free)), f"goal moved ({goal.x}, {goal.y}) -> ({free.x}, {free.y})"
+        except Exception as e:  # noqa: BLE001 - a failed recovery must never take the server down
+            return False, f"{type(e).__name__}: {e}"
+        return False, f"unknown action {a.kind}"
+
+    def _log(self, f: Fault, a: Action, success: bool, note: str = "") -> dict:
+        ev = HealEvent(timestamp=round(self._clock(), 3), error_signature=f"{f.kind}: {f.signature}",
+                       action_taken=f"{a.kind} {a.target}".strip(), success=success)
+        rec = {**ev.model_dump(), "evidence": f.evidence, "note": note}
+        self.events.append(rec)
+        try:
+            self.log_path.parent.mkdir(parents=True, exist_ok=True)
+            with open(self.log_path, "a") as fh:
+                fh.write(json.dumps(rec) + "\n")
+        except OSError:
+            pass
+        return rec
+
+
+# ---------------------------------------------------------------------------
+# Container-side node control (used by the API server)
+# ---------------------------------------------------------------------------
+
+class NodeControl:
+    """Find and restart the nodes the healer knows how to replace."""
+
+    def __init__(self, run_dir: Path, robot: str = "waffle") -> None:
+        self.run_dir = Path(run_dir)
+        self.robot = robot
+        self._procs: List[subprocess.Popen] = []
+        self._lock = threading.Lock()
+
+    @staticmethod
+    def _pgrep(pattern: str) -> bool:
+        return subprocess.run(["pgrep", "-f", pattern], capture_output=True).returncode == 0
+
+    def alive(self) -> Dict[str, bool]:
+        return {name: self._pgrep(f"lib/{pkg}/{exe}") for name, (pkg, exe) in NODES.items()}
+
+    def _command(self, name: str) -> List[str]:
+        share = f"/opt/ros/{os.getenv('ROS_DISTRO', 'jazzy')}/share/turtlebot3_gazebo"
+        model = f"turtlebot3_{self.robot}"
+        if name == "robot_state_publisher":
+            import yaml  # in the sim image (ROS dependency)
+
+            urdf = Path(f"{share}/urdf/{model}.urdf").read_text()
+            params = self.run_dir / "logs" / "rsp_params.l2s.yaml"
+            params.write_text(yaml.safe_dump({"robot_state_publisher": {"ros__parameters": {
+                "use_sim_time": True, "robot_description": urdf}}}))
+            return ["ros2", "run", "robot_state_publisher", "robot_state_publisher",
+                    "--ros-args", "--params-file", str(params)]
+        if name == "bridge":
+            return ["ros2", "run", "ros_gz_bridge", "parameter_bridge", "--ros-args",
+                    "-p", f"config_file:={share}/params/{model}_bridge.yaml", "-p", "use_sim_time:=true"]
+        raise ValueError(f"no restart recipe for {name}")
+
+    def restart(self, name: str) -> bool:
+        pkg, exe = NODES[name]
+        subprocess.run(["pkill", "-f", f"lib/{pkg}/{exe}"], capture_output=True)
+        with self._lock:
+            log = open(self.run_dir / "logs" / f"healed_{name}.log", "a")
+            self._procs.append(subprocess.Popen(self._command(name), stdout=log, stderr=subprocess.STDOUT,
+                                                start_new_session=True))
+        for _ in range(50):
+            if self.alive().get(name):
+                return True
+            time.sleep(0.1)
+        return False
+
+    def stop_all(self) -> None:
+        with self._lock:
+            for p in self._procs:
+                if p.poll() is None:
+                    try:
+                        os.killpg(p.pid, 2)
+                        p.wait(timeout=5)
+                    except Exception:  # noqa: BLE001
+                        p.kill()
+            self._procs.clear()