ros_contorl/scripts/rdk_release_movel.py

#!/usr/bin/env python3
"""D435 这一点用厂家 MoveL 送到工具尖，再把真机控制栈拉回来。

RDK 同时只能有一个连接。混合流程里 `run_real.sh` 已经占着它，
所以顺序是：停真机栈 → MoveL（`--keep-z --go`）→ 用 `--no-stop` 把栈拉回来。

`--no-stop` 是为了不扫 `install/`：感知和伺服还在跑，扫了会把自己杀掉。
YOLO 不在这里关。
"""

from __future__ import annotations

import argparse
import os
import signal
import subprocess
import sys
import time
from pathlib import Path

ROOT = Path(__file__).resolve().parents[1]
sys.path.insert(0, str(ROOT / "scripts" / "lib"))

from rdk_release import is_control_stack_cmd  # noqa: E402

RDK_PYTHON = os.environ.get("RDK_PYTHON", "/home/cc/rdk_py310/bin/python")


def _procs() -> list[tuple[int, str]]:
    out: list[tuple[int, str]] = []
    proc = Path("/proc")
    for entry in proc.iterdir():
        if not entry.name.isdigit():
            continue
        pid = int(entry.name)
        if pid == os.getpid():
            continue
        try:
            raw = (entry / "cmdline").read_bytes().replace(b"\x00", b" ")
        except OSError:
            continue
        cmd = raw.decode("utf-8", "replace").strip()
        if cmd:
            out.append((pid, cmd))
    return out


def _control_pids() -> list[tuple[int, str]]:
    return [(pid, cmd) for pid, cmd in _procs() if is_control_stack_cmd(cmd)]


def _term(pids: list[int]) -> None:
    for pid in pids:
        try:
            os.kill(pid, signal.SIGTERM)
        except OSError:
            pass
    deadline = time.monotonic() + 8.0
    while time.monotonic() < deadline:
        alive = []
        for pid in pids:
            try:
                os.kill(pid, 0)
            except OSError:
                continue
            alive.append(pid)
        if not alive:
            return
        time.sleep(0.2)
        pids = alive
    for pid in pids:
        try:
            os.kill(pid, signal.SIGKILL)
        except OSError:
            pass


def _service_up() -> bool:
    try:
        listed = subprocess.run(
            ["timeout", "3", "ros2", "service", "list"],
            capture_output=True,
            text=True,
            check=False,
        )
    except OSError:
        return False
    return "/controller_manager/list_controllers" in (listed.stdout or "")


def _controller_active() -> bool:
    if not _service_up():
        return False
    try:
        call = subprocess.run(
            [
                "timeout", "3", "ros2", "service", "call",
                "/controller_manager/list_controllers",
                "controller_manager_msgs/srv/ListControllers",
            ],
            capture_output=True,
            text=True,
            check=False,
        )
    except OSError:
        return False
    text = (call.stdout or "") + (call.stderr or "")
    return "name='rizon_arm_controller', state='active'" in text


def _wait_controller(up: bool, timeout_s: float) -> bool:
    deadline = time.monotonic() + timeout_s
    while time.monotonic() < deadline:
        active = _controller_active()
        if active == up:
            return True
        time.sleep(0.5)
    return _controller_active() == up


def main() -> int:
    ap = argparse.ArgumentParser()
    ap.add_argument("--xyz", nargs=3, type=float, required=True, metavar=("X", "Y", "Z"))
    ap.add_argument("--arm", default=os.environ.get("SITE_ROBOT_ARM", "89"))
    ap.add_argument("--vel", type=float, default=0.3, help="TCP 线速度 m/s，交给 SDK vel")
    ap.add_argument("--acc", type=float, default=1.5, help="TCP 加速度 m/s²，交给 SDK acc")
    args = ap.parse_args()

    held = _control_pids()
    if held:
        print(f"放开 RDK：停 {len(held)} 个真机控制进程（感知不停）")
        for pid, cmd in held:
            print(f"  {pid} {cmd[:160]}")
        _term([pid for pid, _cmd in held])
        if not _wait_controller(False, 20.0):
            print("ERROR: 控制器服务还在，MoveL 会和它抢 RDK", file=sys.stderr)
            return 1
    else:
        print("真机控制栈没在跑，直接 MoveL")

    x, y, z = (float(v) for v in args.xyz)
    movel = [
        RDK_PYTHON,
        str(ROOT / "scripts" / "rdk_goto_xyz.py"),
        "--xyz", f"{x}", f"{y}", f"{z}",
        "--keep-z", "--go",
        "--vel", f"{float(args.vel)}",
        "--acc", f"{float(args.acc)}",
    ]
    print("厂家 MoveL：XY 对准 D435 这一点，工具尖高度保持当前")
    got = subprocess.run(movel, check=False)
    if got.returncode != 0:
        print(f"ERROR: MoveL 退出码 {got.returncode}", file=sys.stderr)

    if held:
        log_path = Path("/tmp/run_all_real_restart.log")
        print(f"拉回真机栈（--no-stop，不扫感知） log={log_path}")
        log_f = log_path.open("ab")
        subprocess.Popen(
            ["bash", str(ROOT / "scripts" / "run_real.sh"), "--no-stop", "--arm", str(args.arm)],
            stdout=log_f,
            stderr=subprocess.STDOUT,
            start_new_session=True,
        )
        if not _wait_controller(True, 120.0):
            print(
                "ERROR: 控制栈没回到 ACTIVE。看 /tmp/run_all_real_restart.log",
                file=sys.stderr,
            )
            return 1
        print("rizon_arm_controller 已回到 ACTIVE")
    return 0 if got.returncode == 0 else int(got.returncode)


if __name__ == "__main__":
    raise SystemExit(main())