p49_process_supervisor.py

"""Spawn and manage ROS node worker subprocesses from the visualizer app."""
from __future__ import annotations

import logging
import os
import signal
import subprocess
import sys
import time
from dataclasses import dataclass, field
from typing import Optional

log = logging.getLogger(__name__)

ROLE_MODULES = {
    "world": "avlite.plugins.avlite_executer_ROS2.p45_world_node",
    "planner": "avlite.plugins.avlite_executer_ROS2.p43_planner_node",
    "controller": "avlite.plugins.avlite_executer_ROS2.p44_controller_node",
    "perception": "avlite.plugins.avlite_executer_ROS2.p42_perception_node",
}

DEFAULT_ROLES = ("planner", "controller", "perception")
EXTERNAL_WORLD_ROLES = ("world", "planner", "controller", "perception")

_STALE_WORKER_PATTERN = "avlite.plugins.avlite_executer_ROS2.p4"


def session_ros_domain_id() -> str:
    """ROS DDS domain: default 0; honor ROS_DOMAIN_ID when set in the environment."""
    existing = os.environ.get("ROS_DOMAIN_ID")
    if existing is not None and existing.strip().isdigit():
        return existing.strip()
    return "0"


def _kill_stale_workers(tracked_pids: set[int]) -> None:
    """Terminate orphan AVLite ROS worker processes from prior sessions."""
    try:
        out = subprocess.check_output(
            ["pgrep", "-f", _STALE_WORKER_PATTERN],
            text=True,
        )
    except subprocess.CalledProcessError:
        return
    except FileNotFoundError:
        log.debug("pgrep unavailable; skipping stale worker cleanup")
        return

    stale_pids: list[int] = []
    for line in out.strip().splitlines():
        if not line.strip():
            continue
        pid = int(line.strip())
        if pid in tracked_pids or pid == os.getpid():
            continue
        stale_pids.append(pid)

    if not stale_pids:
        return

    for pid in stale_pids:
        log.info("Stopping stale ROS worker pid=%s", pid)
        try:
            os.killpg(os.getpgid(pid), signal.SIGTERM)
        except ProcessLookupError:
            pass
        except OSError:
            try:
                os.kill(pid, signal.SIGTERM)
            except ProcessLookupError:
                pass

    time.sleep(0.5)

    for pid in stale_pids:
        try:
            os.kill(pid, 0)
        except ProcessLookupError:
            continue
        log.warning("Force-killing stale ROS worker pid=%s", pid)
        try:
            os.killpg(os.getpgid(pid), signal.SIGKILL)
        except ProcessLookupError:
            pass
        except OSError:
            try:
                os.kill(pid, signal.SIGKILL)
            except ProcessLookupError:
                pass


@dataclass
class ROSProcessSupervisor:
    profile: str = "default"
    roles: tuple[str, ...] = DEFAULT_ROLES
    _processes: list[subprocess.Popen] = field(default_factory=list)

    def start(
        self,
        profile: Optional[str] = None,
        roles: Optional[list[str]] = None,
        compute_planner: Optional[str] = None,
        compute_controller: Optional[str] = None,
    ) -> None:
        self.stop()
        tracked = {p.pid for p in self._processes if p.poll() is None}
        _kill_stale_workers(tracked)

        if profile is not None:
            self.profile = profile
        if roles is not None:
            self.roles = tuple(roles)

        env = os.environ.copy()
        env["AVLITE_PROFILE"] = self.profile
        if compute_planner:
            env["AVLITE_LOCAL_PLANNER"] = compute_planner
        if compute_controller:
            env["AVLITE_CONTROLLER"] = compute_controller

        # Live GUI map/plan (may differ from saved profile YAML).
        from avlite.c40_execution.c49_settings import ExecutionSettings

        if ExecutionSettings.c40_map:
            env["AVLITE_MAP"] = ExecutionSettings.c40_map
        else:
            env.pop("AVLITE_MAP", None)
        if ExecutionSettings.c40_global_trajectory:
            env["AVLITE_GLOBAL_TRAJECTORY"] = ExecutionSettings.c40_global_trajectory
        else:
            env.pop("AVLITE_GLOBAL_TRAJECTORY", None)

        for role in self.roles:
            module = ROLE_MODULES.get(role)
            if module is None:
                log.error("Unknown ROS worker role: %s", role)
                continue
            cmd = [sys.executable, "-m", module, "--profile", self.profile]
            log.info("Starting ROS worker %s: %s", role, " ".join(cmd))
            proc = subprocess.Popen(
                cmd,
                env=env,
                start_new_session=True,
            )
            self._processes.append(proc)

    def stop(self) -> None:
        for proc in self._processes:
            if proc.poll() is not None:
                continue
            try:
                os.killpg(os.getpgid(proc.pid), signal.SIGTERM)
            except ProcessLookupError:
                pass
            except Exception as e:
                log.debug("SIGTERM worker pid=%s: %s", proc.pid, e)

        for proc in self._processes:
            try:
                proc.wait(timeout=3.0)
            except subprocess.TimeoutExpired:
                try:
                    os.killpg(os.getpgid(proc.pid), signal.SIGKILL)
                except ProcessLookupError:
                    pass
                proc.wait(timeout=1.0)

        self._processes.clear()

    def is_alive(self) -> bool:
        if not self._processes:
            return False
        return any(p.poll() is None for p in self._processes)