克隆下来必须带上正在用的视觉权重和本机 site.yaml，否则感知只能指到一台机器上的路径，别人起不来。

顶视迁臂改走厂家 MoveL，D405 框心对准后再对孔；夹爪停在 caught 时 open 不再把相同目标当成已经张开，init 会真的回零。 Co-authored-by: Cursor <cursoragent@cursor.com>
2026-10-12 17:08:43 +08:00
parent b903161113
commit f882c4309c
187 changed files with 22898 additions and 104 deletions
@@ -0,0 +1,174 @@
+#!/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.05)
+    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)}",
+    ]
+    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())