arm_dev
7942014763
验收：test_listed_arms 与 test_camera_preview 共 20 项通过。D435 1280×720 发布约 29 帧/秒。 Co-authored-by: Cursor <cursoragent@cursor.com>
230 lines
8.4 KiB
Python
230 lines
8.4 KiB
Python
#!/usr/bin/env python3
|
||
"""读一次机械臂的"能不能动"状态，并给出**人话结论**。
|
||
|
||
必须在 ``/home/cc/rdk_py310/bin/python`` 下跑（flexivrdk 与 ROS 的 3.11 ABI 不兼容）。
|
||
|
||
为什么要有这个脚本
|
||
------------------
|
||
``run_real.sh`` 起来之后，如果机器人没使能，``ros2_control`` 会**卡在**
|
||
``on_activate`` 里等 ``operational``（``wait_for_operational`` 是死循环重试）。
|
||
卡住的表现是：
|
||
|
||
· ``ros2 control list_controllers`` → ``Could not contact service``（不是"没起来"）
|
||
· ``/joint_states`` 一条都不发
|
||
· 日志里每 5 秒一句 ``Waiting for operational (status: ...)``
|
||
|
||
实测（2026-09-28）：状态是 ``IN_MANUAL_MODE`` —— 注意 ``robot.mode()`` 返回的是
|
||
``Mode.IDLE``，**看不出**这点，必须读 ``operational_status()``。示教器那个 Auto/Manual
|
||
开关没有 RDK 接口可以远程切（Flexiv 的安全设计），所以只能人到现场拨。
|
||
这个脚本的价值就是把"卡住了"翻译成"去拨那个开关"，而不是让人去查 ROS。
|
||
|
||
输出
|
||
----
|
||
``--out FILE`` 写 ``KEY=VALUE``（RDK 自己的日志会混在 stdout 里，所以结果单独落文件，
|
||
不去解析第三方日志）。失败分类用 :func:`classify_state`（纯函数，可单测）。
|
||
"""
|
||
|
||
from __future__ import annotations
|
||
|
||
import argparse
|
||
import json
|
||
import sys
|
||
from pathlib import Path
|
||
from typing import Dict, List, Optional, Sequence, Tuple
|
||
|
||
# rdk_status.py 必须在 **3.10** 下跑（flexivrdk 与 ROS 的 3.11 ABI 不兼容），
|
||
# 所以这里只 import 标准库 + 同目录下同样只用标准库的 manual_actions。
|
||
sys.path.insert(0, str(Path(__file__).resolve().parent))
|
||
|
||
from manual_actions import ( # noqa: E402
|
||
NET_UNREACHABLE_ACTION,
|
||
RDK_REMOTE_MODE_ACTION,
|
||
RDK_STACK_HOLDING_ACTION,
|
||
)
|
||
|
||
# 结论级别：ok / warn / fail（与 scripts/lib/usb_health.py 同一套词）
|
||
LEVEL_OK = "ok"
|
||
LEVEL_WARN = "warn"
|
||
LEVEL_FAIL = "fail"
|
||
|
||
# 现场那句"必须人去拨"的说明。preflight 与文档引用同一句话。
|
||
MANUAL_HINT = (
|
||
"示教器切到 Auto（Remote）—— RDK **没有**远程切 Manual→Auto 的接口，"
|
||
"这是 Flexiv 的安全设计，必须人到现场拨。切完**不用**重启栈："
|
||
"run_real.sh 里的 wait_for_operational 会自动接着 Enable。"
|
||
)
|
||
|
||
|
||
def classify_state(
|
||
*,
|
||
connected: bool,
|
||
operational: bool = False,
|
||
op_status: str = "",
|
||
mode: str = "",
|
||
fault: bool = False,
|
||
estop_released: bool = True,
|
||
error: str = "",
|
||
) -> Tuple[str, str, List[str]]:
|
||
"""把机器人状态翻成 ``(级别, 一句话结论, 建议动作)``。
|
||
|
||
判定顺序是**故意的**：故障/急停要排在模式前面 —— 急停没复位时模式可能还是
|
||
Auto，只报"模式不对"会让人白跑一趟。
|
||
"""
|
||
if not connected:
|
||
return (
|
||
LEVEL_FAIL,
|
||
f"连不上 RDK：{error or '未知原因'}",
|
||
[
|
||
# 措辞来自 manual_actions：体检报告里 check_network 也会给出
|
||
# 同一条，两处字面一致才能去重成一条（否则人读两遍）。
|
||
NET_UNREACHABLE_ACTION,
|
||
RDK_REMOTE_MODE_ACTION,
|
||
RDK_STACK_HOLDING_ACTION,
|
||
],
|
||
)
|
||
if fault:
|
||
return (
|
||
LEVEL_FAIL,
|
||
"机器人有故障（fault）",
|
||
[
|
||
"在示教器上清故障；没有示教器就重启控制柜。",
|
||
"栈里的 wait_for_operational 也会自动 ClearFault 重试。",
|
||
],
|
||
)
|
||
if not estop_released:
|
||
return (
|
||
LEVEL_FAIL,
|
||
"急停没复位（estop_released=False）",
|
||
["松开示教器/控制柜上的急停按钮，再按面板上的复位。"],
|
||
)
|
||
upper = (op_status or "").upper()
|
||
if "MANUAL" in upper:
|
||
return (
|
||
LEVEL_FAIL,
|
||
f"示教器在 Manual 模式（operational_status={op_status}）—— "
|
||
"机器人不会使能，整条流程会一直等着",
|
||
[MANUAL_HINT],
|
||
)
|
||
if operational:
|
||
return (LEVEL_OK, f"已使能、可接收指令（mode={mode}）", [])
|
||
return (
|
||
LEVEL_FAIL,
|
||
f"未使能（operational_status={op_status or '?'}, mode={mode or '?'}）",
|
||
[
|
||
MANUAL_HINT,
|
||
"确认没有未清的故障、急停已复位。",
|
||
],
|
||
)
|
||
|
||
|
||
def pack_status(state: Dict[str, object]) -> Dict[str, object]:
|
||
"""把探针结果收成给界面的 JSON。
|
||
|
||
``q`` 不能传进 ``classify_state``：那个函数只认使能字段。
|
||
曾经用 ``classify_state(**state)``，关节角一读到就抛 TypeError，
|
||
JSON 根本没打出来，界面就永远停在“还没连上”。
|
||
"""
|
||
fields = dict(state)
|
||
q = fields.pop("q", None)
|
||
level, summary, actions = classify_state(**fields) # type: ignore[arg-type]
|
||
payload: Dict[str, object] = {
|
||
**fields,
|
||
"level": level,
|
||
"summary": summary,
|
||
"actions": actions,
|
||
}
|
||
if isinstance(q, list) and len(q) == 7:
|
||
payload["q"] = q
|
||
return payload
|
||
|
||
|
||
def _open_robot(sn: str, iface: str, pc_ip: str):
|
||
"""按 IPv4 白名单连机器人。网卡名不能直接交给 RDK。"""
|
||
import flexivrdk
|
||
from rdk_helpers import rdk_whitelist_lists
|
||
|
||
# 空名单才退回不指定网卡。指定了 eth0 这种名字时，FastDDS 会把真地址滤掉，
|
||
# 表现是 ping 得通、这里一直搜到超时。
|
||
candidates = rdk_whitelist_lists(iface, pc_ip) or [[]]
|
||
last = ""
|
||
for listen in candidates:
|
||
try:
|
||
if listen:
|
||
return flexivrdk.Robot(sn, list(listen), True, False), ""
|
||
return flexivrdk.Robot(sn), ""
|
||
except Exception as exc: # noqa: BLE001
|
||
last = str(exc)
|
||
return None, last or "连不上 RDK"
|
||
|
||
|
||
def _probe(sn: str, iface: str, enable: bool, pc_ip: str = "") -> Dict[str, object]:
|
||
"""连一次机器人并读状态。返回给 classify_state 的关键字参数，另加关节角 ``q``。"""
|
||
out: Dict[str, object] = {
|
||
"connected": False,
|
||
"operational": False,
|
||
"op_status": "",
|
||
"mode": "",
|
||
"fault": False,
|
||
"estop_released": True,
|
||
"error": "",
|
||
}
|
||
robot, error = _open_robot(sn, iface, pc_ip)
|
||
if robot is None:
|
||
out["error"] = error
|
||
return out
|
||
|
||
out["connected"] = True
|
||
if enable:
|
||
try:
|
||
robot.Enable()
|
||
except Exception as exc: # noqa: BLE001
|
||
out["error"] = f"Enable 失败: {exc}"
|
||
|
||
def _safe(fn, default):
|
||
try:
|
||
return fn()
|
||
except Exception: # noqa: BLE001
|
||
return default
|
||
|
||
out["mode"] = str(_safe(robot.mode, ""))
|
||
out["operational"] = bool(_safe(robot.operational, False))
|
||
out["op_status"] = str(_safe(robot.operational_status, ""))
|
||
out["fault"] = bool(_safe(robot.fault, False))
|
||
out["estop_released"] = bool(_safe(robot.estop_released, True))
|
||
# 只读关节角，给界面画姿态。读不到就空着，绝不在这里 Enable。
|
||
try:
|
||
q = [float(v) for v in list(robot.states().q)[:7]]
|
||
except Exception: # noqa: BLE001
|
||
q = []
|
||
if len(q) == 7 and all(v == v for v in q):
|
||
out["q"] = q
|
||
return out
|
||
|
||
|
||
def main(argv: Optional[Sequence[str]] = None) -> int:
|
||
ap = argparse.ArgumentParser(description="读一次机械臂状态并给结论")
|
||
ap.add_argument("--sn", required=True)
|
||
ap.add_argument("--iface", default="")
|
||
ap.add_argument("--pc-ip", default="", help="本机机器人网卡的 IPv4，用来做 RDK 白名单")
|
||
ap.add_argument("--enable", action="store_true", help="顺手尝试 Enable（会真的使能）")
|
||
ap.add_argument("--out", default="", help="把 KEY=VALUE 写到这个文件")
|
||
ap.add_argument("--json", action="store_true", help="把结果打到 stdout（JSON）")
|
||
args = ap.parse_args(argv)
|
||
|
||
state = _probe(args.sn, args.iface, args.enable, args.pc_ip)
|
||
payload = pack_status(state)
|
||
level = str(payload["level"])
|
||
actions = list(payload["actions"])
|
||
|
||
if args.out:
|
||
lines = [f"{k}={v}" for k, v in payload.items() if k != "actions"]
|
||
lines.append("actions=" + " | ".join(str(item) for item in actions))
|
||
Path(args.out).write_text("\n".join(lines) + "\n", encoding="utf-8")
|
||
if args.json:
|
||
print(json.dumps(payload, ensure_ascii=False, indent=2))
|
||
return 0 if level == LEVEL_OK else 1
|
||
|
||
|
||
if __name__ == "__main__":
|
||
sys.exit(main())
|