P3b:手眼标定支持 --mode 模式选择与 --points 目标点数

--mode eye_in_hand|eye_to_hand
- 切换读写的文件为 <臂号>_<模式>.yaml,两种模式的采点与结果互不覆盖、可共存
- 臂号从 Flexiv 序列号后两位取(Rizon4-063989 → 89),不做硬编码映射
- 非法模式立即报错退出。不静默退回默认值 —— 那会造成"以为标的是
  eye-to-hand、其实算的是 eye-in-hand",是这类工具最难排查的错误
- 求解侧本来就支持(calibrate_hand_eye(eye_to_hand=...) 对 gripper2base 求逆),
  这次把入口补上,让模式不再只存在于参数文件里

--points N(仅 teach)
- teach 新增 target_points 参数并实现计数:显示 [3/12] ... 还差 9 个点,
  采够时提示可以收工
- 刻意不强制关窗:实际采点常出现"某个角度还没覆盖",强制退出要重开

usage 重写:路径改为 handeye/,补上全局选项与组合示例;
usage() 的 sed 行号随之同步(原 2,18p 已不覆盖新头部)。

handeye/README.md 补第 5 节:模式切换与点数的完整用法,
并说明为什么非法模式要报错而不是兜底。

测试:224 passed
This commit is contained in:
arm_dev
2026-09-23 11:48:11 +08:00
parent a3a589b9e3
commit e4f513392d
3 changed files with 127 additions and 10 deletions
@@ -257,6 +257,10 @@ class HandeyeTeach(Node):
self.declare_parameter('emitter_enabled', False)
self.declare_parameter('exposure_us', 0.0)
self.declare_parameter('gain', 0.0)
# 目标采集点数。0 = 不限(按 q 结束)。
# 设了就只是个"提醒":采够时提示可以收工,不强制关窗 ——
# 常常采到一半发现某个角度还没覆盖,强制退出反而要重开。
self.declare_parameter('target_points', 0)
self.backend = str(self.get_parameter('backend').value).strip().lower()
self.robot_sn = str(self.get_parameter('robot_sn').value)
@@ -557,7 +561,29 @@ class HandeyeTeach(Node):
f.write(f'# image {img_path}\n')
print(f'\n# pose {idx} {img_path.name}')
print(line)
self.get_logger().info(f'[{idx + 1}] saved photo+joints')
n = idx + 1
try:
target = int(self.get_parameter('target_points').value)
except Exception: # noqa: BLE001
target = 0
if target > 0:
if n < target:
self.get_logger().info(
f'[{n}/{target}] saved photo+joints —— 还差 {target - n} 个点'
)
elif n == target:
self.get_logger().info(
f'[{n}/{target}] 已采够 {target} 个点。'
' 建议按 q 结束,然后跑 merge + calibrate;'
' 想多采几个点也可以继续按 Enter。'
)
else:
self.get_logger().info(
f'[{n}/{target}] saved photo+joints —— 已超出目标 '
f'{n - target} 个点(会用全部点求解)'
)
else:
self.get_logger().info(f'[{n}] saved photo+joints')
self._write_yaml_snippet()
finally:
self._busy = False