arm_dev
f5ed44a987
## 为什么
"设备 ID 全部可配置"这条要求在代码里只做了一半：site.yaml 能配，
但**代码和静态配置里到处躺着 Rizon4-063989 当兜底**。这类兜底的危害
不是"改起来麻烦"，而是它把"配置没生效"伪装成"配置生效了"：
* `robot_sn` 默认 `Rizon4-063989` → 换臂后节点去订**上一台臂**的关节话题，
节点起来了、不报错、什么都不动；
* launch 里用 `ee_frame == "Rizon4-063989_flange"` 当"用户没指定"的哨兵 →
换个站点这个判断永远不成立，于是 site.yaml 里的 ee_frame 再也不会被用上，
静态 TF 挂错坐标系、点还照样查得到；
* `if SN == "Rizon4-063989"` 决定要不要拉触觉服务 → 换臂后触觉静默消失。
## 改了什么
1. **删掉所有"兜底成某台臂"的默认值**（15 处脚本 + 6 个 launch + 6 个节点）。
没配就为空 / 直接 `:?` 报错，宁可起不来也不要连错机器：
- `scripts/lib/export_site_env.py`：别名解析改为从 `robots[]` 现算
（`arm_short_id` / `resolve_robot_sn` / `describe_arms`），选不出臂时
抛 `ValueError`；`main()` 捕获后打人话 + 退出码 2（供 shell `eval`）；
- 手眼文件/路点/工作目录由当前臂 short id 派生，不再写死 `89_*`；
- `ROBOT_HAS_TACTILE` 改为读 site.yaml 的 `robots[].tactile`；
- `arm_application/site_io.py` 新增 `joint_prefix_default()` /
`base_frame_default()` / `ee_frame_default()`：优先读 `SITE_*`，
退而由 `SITE_ROBOT_SN` 现算，**不写死 SN**；
- `arm.urdf.xacro` 的 `robot_sn` 默认值改成明显的占位符 `UNSET_ARM_SN`
（关节名会带出来，报错信息自解释）；
- `ros2_control.launch.py` / `move_group.launch.py` **原先根本不传
robot_sn**，靠 xacro 写死默认值"蒙对"本机，现在都从环境取。
2. **新增 `scripts/check_arm_prefixes.py`**：Flexiv 把 SN 编进关节名，
所以有一批一级配置（SRDF/controllers/joint_names/bringup/servo/...）
必须跟着当前臂走。漏改的症状是"MoveIt 规划成功但机械臂不动"，很难查。
这个脚本逐文件逐行列出对不上的地方，并直接给出修法。
`--list` 还会说明 SN 来自 site.yaml 还是被 `ROBOT_SN` 覆盖。
3. **文档**：`FRAMEWORK_RULES.md` 新增 R1.1.1（两类含臂名文件怎么分、
换臂四步）+ 检查清单第 12 条 + 反例 2 条；`HANDBOOK.md` 新增 3.6.1
与症状表 2 行。
4. **顺带修掉的真 bug**（都是这次改动带出来、被 pyflakes + 真跑抓住的）：
- `convert_flexiv_traj.py` / `rdk_drag_bridge.py` 用了 `os` 但没 import
→ `--help` 直接 NameError；
- `check_arm_prefixes.py` 第一版正则用 `\b` 收尾，而关节名是
`Rizon4-063989_joint1`（`_` 属于 \w，`\b` 不成立）→ SRDF 里 24 处
硬编码**一处都没报**，还打印 OK。已加负向测试钉住；
- 删掉若干死代码/死 import（`pos_line`/`flat`/`names`、`IfCondition` 等）。
## 验收（都真跑过）
- `python3 -m pyflakes <全部改过的 py>` → 干净
- 逐包 pytest：233 + 69 + 78 + 24 + 236 = **640 passed**
- `env -u ROBOT_SN python3 scripts/check_arm_prefixes.py` → OK（8 文件）
- 换成 SN=Rizon4-777001 的临时 site.yaml → 报出**8 个**文件不一致并给修法（退出码 1）
- 故意不给 SITE_* → 各 launch 的 `robot_sn` / `ee_frame` / `handeye_file`
默认值为 `''`，不再悄悄指向上一台臂
- `ros2 launch ... --show-args` 对 stack/sim/ros2_control/mock_control/
move_group/display/visual_servo **全部通过**
- `convert_flexiv_traj.py --help` 正常；拿 `xunhuan.traj` 真转 5 个路点，
输出 `joint_prefix: "Rizon4-063989_"` 来自 site.yaml
Co-authored-by: Cursor <cursoragent@cursor.com>
441 lines
17 KiB
Python
Executable File
441 lines
17 KiB
Python
Executable File
#!/usr/bin/env python3
|
||
"""RDK 控制桥（必须在 rdk_py310 / Python 3.10 下运行）。
|
||
|
||
为什么需要这个进程
|
||
------------------
|
||
非夕的 `flexivrdk` Python 绑定与 ROS 环境的 Python 3.11 ABI 不兼容，且 RDK 对
|
||
同一台机器人是**独占访问**：多个进程同时连同一个 SN 会失败。所以框架里所有
|
||
RDK 操作都收敛到本进程，由它串行化，业务节点通过 JSON 行协议远程调用。
|
||
|
||
这也让 `arm_driver_flexiv` 不必 import flexivrdk，符合
|
||
`docs/INTERFACES.md` 的约束（业务节点不得 import flexivrdk）。
|
||
|
||
软拖拽为什么用 NRT_JOINT_IMPEDANCE
|
||
----------------------------------
|
||
AUTO(Remote) 模式下示教器上的 Free Drive 按钮不可用（控制权在 Remote）。
|
||
RDK 的 FloatingJoint/FloatingCartesian 通常需要额外授权。
|
||
所以默认用低刚度 NRT_JOINT_IMPEDANCE：目标跟踪实测关节角 + 低刚度，
|
||
于是手臂可以用手推动，同时 Remote 保持开启。
|
||
|
||
线程安全
|
||
--------
|
||
`flexivrdk.Robot` 不是线程安全的。所有 robot API 调用（含拖拽循环）共用同一
|
||
把锁 `rlock`。拖拽期间的关节读取走缓存，避免 Enter 与 Stop() 竞争。
|
||
|
||
行协议（JSON 只写 stdout；RDK 自身日志走 stderr）
|
||
-------------------------------------------------
|
||
{"cmd":"ping"}
|
||
{"cmd":"connect","sn":"<site.yaml 的 robot.sn>","iface":"<robot.iface>"}
|
||
{"cmd":"enable"} # 使能 + 清故障（幂等）
|
||
{"cmd":"clear_fault"} # 显式清故障并使能
|
||
{"cmd":"switch_tool","name":"tool1"} # 切工具；name 空=只查询
|
||
{"cmd":"status"} # operational/fault/mode/tool
|
||
{"cmd":"drag_on","mode":"soft"} # soft | floating
|
||
{"cmd":"drag_off"}
|
||
{"cmd":"joints"}
|
||
{"cmd":"quit"}
|
||
|
||
每个命令的应答都是单行 JSON，含 ``ok`` 字段；失败时另有 ``error``。
|
||
"""
|
||
|
||
from __future__ import annotations
|
||
|
||
import json
|
||
import os
|
||
import sys
|
||
import threading
|
||
import time
|
||
from pathlib import Path
|
||
from typing import List, Optional
|
||
|
||
# 公共 RDK 操作统一在 scripts/lib/rdk_helpers.py，避免与 rdk_switch_tool.py
|
||
# 各写一份导致行为漂移。
|
||
sys.path.insert(0, str(Path(__file__).resolve().parent / 'lib'))
|
||
from rdk_helpers import ( # noqa: E402
|
||
clear_fault as rdk_clear_fault,
|
||
ensure_idle,
|
||
ensure_operational,
|
||
resolve_tool_name,
|
||
single_arm_groups,
|
||
switch_tool,
|
||
tool_name,
|
||
tool_params,
|
||
)
|
||
|
||
_JSON_OUT = sys.stdout
|
||
sys.stdout = sys.stderr
|
||
|
||
|
||
def reply(obj: dict) -> None:
|
||
_JSON_OUT.write(json.dumps(obj, ensure_ascii=False) + '\n')
|
||
_JSON_OUT.flush()
|
||
|
||
|
||
def log(msg: str) -> None:
|
||
print(f'[rdk_bridge] {msg}', file=sys.stderr, flush=True)
|
||
|
||
|
||
class SoftDrag:
|
||
def __init__(self, robot, flexivrdk, lock: threading.Lock) -> None:
|
||
self.robot = robot
|
||
self.flexivrdk = flexivrdk
|
||
self._lock = lock
|
||
self._stop = threading.Event()
|
||
self._thread: Optional[threading.Thread] = None
|
||
self.latest_q: List[float] = []
|
||
self._groups = {}
|
||
|
||
def start(self) -> None:
|
||
mode = self.flexivrdk.Mode
|
||
robot = self.robot
|
||
with self._lock:
|
||
robot.SwitchMode(mode.NRT_JOINT_IMPEDANCE)
|
||
st = robot.states()
|
||
self.latest_q = [float(v) for v in list(st.q)[:7]]
|
||
try:
|
||
info = robot.info()
|
||
self._groups = dict(getattr(info, 'single_arm_groups', None) or {})
|
||
except Exception as exc:
|
||
log(f'info.single_arm_groups: {exc}')
|
||
self._groups = {}
|
||
if self._groups:
|
||
for g in self._groups:
|
||
try:
|
||
nom = list(info.K_q_nom[g])
|
||
soft = [max(8.0, 0.08 * float(v)) for v in nom]
|
||
robot.SetJointImpedance(g, soft)
|
||
log(f'SetJointImpedance {g} K={soft}')
|
||
except Exception as exc:
|
||
log(f'SetJointImpedance 组 {g}: {exc}')
|
||
else:
|
||
n = max(7, len(self.latest_q))
|
||
try:
|
||
robot.SetJointImpedance([35.0] * n, [0.7] * n)
|
||
except Exception as exc:
|
||
log(f'SetJointImpedance flat（不分组）: {exc}')
|
||
|
||
self._stop.clear()
|
||
self._thread = threading.Thread(target=self._loop, name='soft-drag', daemon=True)
|
||
self._thread.start()
|
||
|
||
def _loop(self) -> None:
|
||
robot = self.robot
|
||
flexivrdk = self.flexivrdk
|
||
while not self._stop.is_set():
|
||
try:
|
||
with self._lock:
|
||
if self._stop.is_set():
|
||
break
|
||
st = robot.states()
|
||
q = [float(v) for v in list(st.q)[:7]]
|
||
self.latest_q = q
|
||
if self._groups:
|
||
cmds = {}
|
||
for g in self._groups:
|
||
try:
|
||
gq = list(st[g].q)
|
||
except Exception:
|
||
gq = q
|
||
n = len(gq)
|
||
cmds[g] = flexivrdk.NrtJointPositionCmd(
|
||
gq, [0.0] * n, [1.5] * n, [2.0] * n
|
||
)
|
||
robot.SendJointPosition(cmds)
|
||
else:
|
||
n = len(q)
|
||
robot.SendJointPosition(q, [0.0] * n, [1.5] * n, [2.0] * n)
|
||
except Exception as exc:
|
||
log(f'soft 循环: {exc}')
|
||
time.sleep(0.15)
|
||
# 约 20 Hz —— 大部分时间让锁保持释放状态
|
||
for _ in range(10):
|
||
if self._stop.is_set():
|
||
return
|
||
time.sleep(0.005)
|
||
|
||
def stop(self) -> None:
|
||
self._stop.set()
|
||
th = self._thread
|
||
if th is not None:
|
||
th.join(timeout=4.0)
|
||
if th.is_alive():
|
||
log('join 之后 soft 循环仍然存活（无论如何都会切到 IDLE）')
|
||
self._thread = None
|
||
|
||
|
||
def _ensure_operational(robot) -> Optional[str]:
|
||
"""保留旧名以兼容既有调用点；实现已统一到 rdk_helpers.ensure_operational。"""
|
||
return ensure_operational(robot)
|
||
|
||
|
||
def _idle_async(robot, lock: threading.Lock, flexivrdk, timeout: float = 4.0) -> bool:
|
||
"""切到 IDLE，同时避免把 stdin 循环永久阻塞住。"""
|
||
done = threading.Event()
|
||
|
||
def run() -> None:
|
||
try:
|
||
acquired = lock.acquire(timeout=2.0)
|
||
try:
|
||
if acquired:
|
||
try:
|
||
robot.Stop()
|
||
except Exception as exc:
|
||
log(f'Stop: {exc}')
|
||
try:
|
||
robot.SwitchMode(flexivrdk.Mode.IDLE)
|
||
except Exception as exc:
|
||
log(f'SwitchMode IDLE: {exc}')
|
||
finally:
|
||
if acquired:
|
||
lock.release()
|
||
finally:
|
||
done.set()
|
||
|
||
threading.Thread(target=run, name='idle', daemon=True).start()
|
||
ok = done.wait(timeout)
|
||
if not ok:
|
||
log('IDLE/Stop 超时 —— stdin 循环继续运行')
|
||
return ok
|
||
|
||
|
||
def main() -> int:
|
||
import flexivrdk
|
||
|
||
robot = None
|
||
drag_on = False
|
||
soft: Optional[SoftDrag] = None
|
||
rlock = threading.Lock()
|
||
cached_q: List[float] = []
|
||
|
||
for line in sys.stdin:
|
||
line = line.strip()
|
||
if not line:
|
||
continue
|
||
try:
|
||
msg = json.loads(line)
|
||
except Exception as exc: # noqa: BLE001
|
||
reply({'ok': False, 'error': f'JSON 解析失败: {exc}'})
|
||
continue
|
||
|
||
cmd = str(msg.get('cmd', '')).strip()
|
||
try:
|
||
if cmd == 'ping':
|
||
reply({'ok': True, 'py': sys.version.split()[0]})
|
||
|
||
elif cmd == 'connect':
|
||
sn = str(msg.get('sn') or os.environ.get('SITE_ROBOT_SN') or '')
|
||
iface = str(msg.get('iface') or '')
|
||
robot = None
|
||
last_err = None
|
||
attempts = []
|
||
if iface:
|
||
attempts.extend([(sn, [iface]), (sn, [iface], True)])
|
||
attempts.append((sn,))
|
||
for args in attempts:
|
||
try:
|
||
robot = flexivrdk.Robot(*args)
|
||
log(f'已连接 {sn} args={args!r}')
|
||
last_err = None
|
||
break
|
||
except TypeError:
|
||
continue
|
||
except Exception as exc:
|
||
last_err = exc
|
||
log(f'连接 {args!r}: {exc}')
|
||
robot = None
|
||
if robot is None:
|
||
reply({'ok': False, 'error': str(last_err or '连接失败')})
|
||
continue
|
||
err = _ensure_operational(robot)
|
||
if err:
|
||
reply({'ok': False, 'error': err})
|
||
continue
|
||
with rlock:
|
||
q = [float(v) for v in list(robot.states().q)[:7]]
|
||
cached_q = q
|
||
reply({'ok': True, 'sn': sn, 'q': q})
|
||
|
||
elif cmd == 'drag_on':
|
||
if robot is None:
|
||
reply({'ok': False, 'error': '尚未连接'})
|
||
continue
|
||
err = _ensure_operational(robot)
|
||
if err:
|
||
reply({'ok': False, 'error': err})
|
||
continue
|
||
|
||
prefer = str(msg.get('mode', 'soft')).strip().lower()
|
||
if prefer in ('floating', 'primitive', 'floatingjoint'):
|
||
name = str(msg.get('primitive', 'FloatingJoint')).replace('()', '')
|
||
try:
|
||
with rlock:
|
||
robot.SwitchMode(flexivrdk.Mode.NRT_PRIMITIVE_EXECUTION)
|
||
robot.ExecutePrimitive(name, {})
|
||
drag_on = True
|
||
reply({'ok': True, 'drag': True, 'mode': 'floating', 'primitive': name})
|
||
continue
|
||
except Exception as exc:
|
||
log(f'Floating 失败，退回 soft 模式: {exc}')
|
||
|
||
try:
|
||
if soft is not None:
|
||
soft.stop()
|
||
soft = SoftDrag(robot, flexivrdk, rlock)
|
||
soft.start()
|
||
if soft.latest_q:
|
||
cached_q = list(soft.latest_q)
|
||
drag_on = True
|
||
log('软拖拽已开启')
|
||
reply({
|
||
'ok': True,
|
||
'drag': True,
|
||
'mode': 'soft',
|
||
'hint': '轻推机械臂即可，关节阻抗已调低。Enter=拍照（s=保持）。',
|
||
})
|
||
except Exception as exc:
|
||
reply({'ok': False, 'error': f'软拖拽失败: {exc}'})
|
||
|
||
elif cmd == 'drag_off':
|
||
log('收到 drag_off 请求')
|
||
if soft is not None:
|
||
if soft.latest_q:
|
||
cached_q = list(soft.latest_q)
|
||
soft.stop()
|
||
soft = None
|
||
if robot is not None:
|
||
_idle_async(robot, rlock, flexivrdk, timeout=4.0)
|
||
drag_on = False
|
||
log('drag_off 已完成')
|
||
reply({'ok': True, 'drag': False, 'q': cached_q})
|
||
|
||
elif cmd == 'joints':
|
||
q: Optional[List[float]] = None
|
||
if soft is not None and len(soft.latest_q) >= 7:
|
||
q = list(soft.latest_q)
|
||
elif len(cached_q) >= 7:
|
||
q = list(cached_q)
|
||
if q is None or len(q) < 7:
|
||
reply({'ok': False, 'error': '没有关节缓存'})
|
||
continue
|
||
cached_q = q
|
||
reply({'ok': True, 'q': q, 'drag': drag_on})
|
||
|
||
elif cmd == 'enable':
|
||
if robot is None:
|
||
reply({'ok': False, 'error': '尚未连接'})
|
||
continue
|
||
err = ensure_operational(robot)
|
||
reply({
|
||
'ok': err is None,
|
||
'error': err,
|
||
'operational': bool(robot.operational()),
|
||
})
|
||
|
||
elif cmd == 'clear_fault':
|
||
if robot is None:
|
||
reply({'ok': False, 'error': '尚未连接'})
|
||
continue
|
||
err = rdk_clear_fault(robot)
|
||
if err:
|
||
reply({'ok': False, 'error': err})
|
||
continue
|
||
# 清故障后顺手使能，否则调用方还得再发一条 enable。
|
||
err2 = ensure_operational(robot)
|
||
reply({
|
||
'ok': err2 is None,
|
||
'error': err2,
|
||
'fault': bool(robot.fault()),
|
||
'operational': bool(robot.operational()),
|
||
})
|
||
|
||
elif cmd == 'switch_tool':
|
||
if robot is None:
|
||
reply({'ok': False, 'error': '尚未连接'})
|
||
continue
|
||
want = str(msg.get('name', '')).strip()
|
||
|
||
# 切换工具要求 IDLE；这一步是同步的，因为切换本身不能被打断。
|
||
err = ensure_idle(robot, flexivrdk.Mode, timeout_s=12.0)
|
||
if err:
|
||
reply({'ok': False, 'error': err})
|
||
continue
|
||
|
||
try:
|
||
with rlock:
|
||
tool = flexivrdk.Tool(robot)
|
||
groups = single_arm_groups(robot)
|
||
available = [str(n) for n in (tool.list() or [])]
|
||
anchor = groups[0] if groups else None
|
||
|
||
if not want:
|
||
# 纯查询：不切换，把当前工具与参数报回去。
|
||
params = tool_params(tool, anchor)
|
||
reply({
|
||
'ok': True,
|
||
'switched': False,
|
||
'current': tool_name(tool, anchor),
|
||
'available': available,
|
||
**params,
|
||
})
|
||
continue
|
||
|
||
target = resolve_tool_name(tool, want)
|
||
already = all(tool_name(tool, g) == target for g in groups)
|
||
if not already:
|
||
for g in groups:
|
||
switch_tool(tool, g, target)
|
||
time.sleep(0.3)
|
||
|
||
params = tool_params(tool, anchor)
|
||
reply({
|
||
'ok': True,
|
||
'switched': not already,
|
||
'already': already,
|
||
'current': tool_name(tool, anchor),
|
||
'target': target,
|
||
'available': available,
|
||
**params,
|
||
})
|
||
except Exception as exc: # noqa: BLE001
|
||
reply({'ok': False, 'error': str(exc)})
|
||
|
||
elif cmd == 'status':
|
||
if robot is None:
|
||
reply({'ok': False, 'connected': False, 'error': '尚未 connect'})
|
||
continue
|
||
info = {'ok': True, 'connected': True, 'drag': drag_on}
|
||
try:
|
||
info['operational'] = bool(robot.operational())
|
||
info['fault'] = bool(robot.fault())
|
||
info['mode'] = str(robot.mode())
|
||
except Exception as exc: # noqa: BLE001
|
||
info['error'] = str(exc)
|
||
try:
|
||
with rlock:
|
||
tool = flexivrdk.Tool(robot)
|
||
groups = single_arm_groups(robot)
|
||
info['tool'] = tool_name(tool, groups[0] if groups else None)
|
||
except Exception:
|
||
info['tool'] = ''
|
||
reply(info)
|
||
|
||
elif cmd == 'quit':
|
||
if soft is not None:
|
||
soft.stop()
|
||
soft = None
|
||
if robot is not None and drag_on:
|
||
_idle_async(robot, rlock, flexivrdk, timeout=3.0)
|
||
reply({'ok': True})
|
||
return 0
|
||
|
||
else:
|
||
reply({'ok': False, 'error': f'未知命令 {cmd}'})
|
||
except Exception as exc: # noqa: BLE001
|
||
log(f'cmd={cmd} 出错: {exc}')
|
||
reply({'ok': False, 'error': str(exc)})
|
||
return 0
|
||
|
||
|
||
if __name__ == '__main__':
|
||
raise SystemExit(main())
|