From e4f513392d1071855d1a33e7fc30b77a127bb11c Mon Sep 17 00:00:00 2001 From: arm_dev Date: Wed, 23 Sep 2026 11:48:11 +0800 Subject: [PATCH] =?UTF-8?q?P3b=EF=BC=9A=E6=89=8B=E7=9C=BC=E6=A0=87?= =?UTF-8?q?=E5=AE=9A=E6=94=AF=E6=8C=81=20--mode=20=E6=A8=A1=E5=BC=8F?= =?UTF-8?q?=E9=80=89=E6=8B=A9=E4=B8=8E=20--points=20=E7=9B=AE=E6=A0=87?= =?UTF-8?q?=E7=82=B9=E6=95=B0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --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 --- handeye/README.md | 34 +++++++++ scripts/run_handeye.sh | 75 ++++++++++++++++--- .../arm_application/handeye_teach.py | 28 ++++++- 3 files changed, 127 insertions(+), 10 deletions(-) diff --git a/handeye/README.md b/handeye/README.md index 33bc4ba..2788062 100644 --- a/handeye/README.md +++ b/handeye/README.md @@ -160,6 +160,40 @@ bash ~/ros2/scripts/run_handeye.sh show --arm 89 两者共用同一份 `poses/*.yaml`，顺序是 `teach`（采）→ `merge`（存）→ `calibrate`（算）。 +### 切换 eye-in-hand / eye-to-hand + +```bash +# 用腕部相机（默认） +bash ~/ros2/scripts/run_handeye.sh --arm 89 teach + +# 用固定相机 +bash ~/ros2/scripts/run_handeye.sh --arm 89 teach --mode eye_to_hand +bash ~/ros2/scripts/run_handeye.sh --arm 89 merge --mode eye_to_hand +bash ~/ros2/scripts/run_handeye.sh --arm 89 calibrate --mode eye_to_hand +``` + +`--mode` 会把读写的文件切到 `<臂号>_<模式>.yaml`，所以： + +- 两种模式**互不覆盖**，可以分别标定、共存 +- 跑完想切换使用，改 `config/site.yaml` 里 `robots[89].paths.handeye_file` + 指向对应那份即可，代码不用动 + +传了非法模式会立刻报错并退出（不会悄悄退回默认值 —— 那会导致"以为标的是 +eye-to-hand、其实算的是 eye-in-hand"这种最难查的错）。 + +### 设定采集点数 + +```bash +bash ~/ros2/scripts/run_handeye.sh --arm 89 teach --points 12 +``` + +采点过程中会显示 `[3/12] ... 还差 9 个点`，采够时提示可以收工。 +**不会强制关窗** —— 常常采到一半发现某个角度还没覆盖，强制退出反而要重开； +想多采就继续按 `Enter`，想结束按 `q`。 + +不加 `--points` 则不限数量（默认），建议 8～12 个点，至少 5 个。 + + --- ## 6. 结果怎么被用起来 diff --git a/scripts/run_handeye.sh b/scripts/run_handeye.sh index a743e74..cb08546 100755 --- a/scripts/run_handeye.sh +++ b/scripts/run_handeye.sh @@ -1,23 +1,37 @@ #!/usr/bin/env bash # Hand-eye calibration helpers. # Scripts: ~/ros2/scripts/run_handeye.sh -# Package YAML: ~/ros2/src/arm_application/config/handeye_*.yaml -# Site overrides (board / motion): ~/ros2/config/site.yaml → handeye: -# Runtime output: /tmp/handeye_calib/ +# Config & results: ~/ros2/handeye/ ← 见 handeye/README.md +# poses/<臂号>_<模式>.yaml 采点（输入） +# results/<臂号>_<模式>.yaml 标定结果（输出） +# calibrate.yaml 板尺寸 / 运动 / 稳定性参数 +# tmp/ 临时产物 +# Site overrides: ~/ros2/config/site.yaml → handeye: # # Usage: # bash ~/ros2/scripts/run_handeye.sh calibrate # bash ~/ros2/scripts/run_handeye.sh waypoints # bash ~/ros2/scripts/run_handeye.sh teach # RDK 软拖拽（推荐） +# bash ~/ros2/scripts/run_handeye.sh teach --points 12 # 目标采 12 个点 # bash ~/ros2/scripts/run_handeye.sh teach --append # 接着上次点位继续采 # bash ~/ros2/scripts/run_handeye.sh teach --ros # 示教器 Free Drive # bash ~/ros2/scripts/run_handeye.sh merge [taught.txt] -# bash ~/ros2/scripts/run_handeye.sh --arm 91 teach -# bash ~/ros2/scripts/run_handeye.sh --arm 91 calibrate +# bash ~/ros2/scripts/run_handeye.sh show # 看当前配置与点数 +# +# 全局选项（放在子命令前或后都可以）: +# --arm 89|91|left|right 选臂（默认 89） +# --mode eye_in_hand|eye_to_hand +# 选模式。会切换到 <臂号>_<模式>.yaml， +# 所以 eye_in_hand 与 eye_to_hand 的结果互不覆盖 +# --points N 仅 teach：目标采集点数（0=不限） +# +# 例： +# bash ~/ros2/scripts/run_handeye.sh --arm 91 teach --mode eye_to_hand --points 15 # # calibrate / waypoints 需要: # T1: bash ~/ros2/scripts/run_real.sh --arm 89|91 # T2: bash ~/ros2/scripts/run_camera_d405.sh --arm 89|91 --preview + # teach（拖拽）需要: 先停掉 run_real.sh 和相机（RDK + D405 都独占） # 89 与 91 的结果文件、示教点、workdir 完全分开，禁止混用。 @@ -32,7 +46,7 @@ CMD="${1:-}" shift || true usage() { - sed -n '2,18p' "$0" + sed -n '2,36p' "$0" } # shellcheck disable=SC1091 @@ -59,6 +73,41 @@ TAUGHT="${WORKDIR}/taught_poses.txt" PREFIX="${SITE_JOINT_PREFIX:-Rizon4-063989_}" CAM="${SITE_CAM_NAME:-d405}" +# ---- --mode：切换 eye-in-hand / eye-to-hand ---- +# 必须在 WP/RES 定下来之前处理，因为它会改变读写的文件名。 +# 其余参数原样留给子命令（teach 自己有 --rdk/--ros 等）。 +HANDEYE_MODE="${SITE_HANDEYE_MODE:-eye_in_hand}" +_rest=() +while [[ $# -gt 0 ]]; do + case "$1" in + --mode) + HANDEYE_MODE="${2:?--mode 需要 eye_in_hand 或 eye_to_hand}" + if [[ "${HANDEYE_MODE}" != "eye_in_hand" && "${HANDEYE_MODE}" != "eye_to_hand" ]]; then + echo "ERROR: --mode 只能是 eye_in_hand 或 eye_to_hand，收到 '${HANDEYE_MODE}'" >&2 + exit 1 + fi + shift 2 + ;; + *) + _rest+=("$1") + shift + ;; + esac +done +set -- "${_rest[@]}" + +# 臂号取 Flexiv 序列号后两位：Rizon4-063989 → 89 +_arm_id="${SITE_ROBOT_SN: -2}" +if [[ -n "${_arm_id}" && "${_arm_id}" != "${SITE_ROBOT_SN}" ]]; then + if [[ "${HANDEYE_MODE}" != "${SITE_HANDEYE_MODE:-eye_in_hand}" ]]; then + # 显式指定了非默认模式 → 按 <臂号>_<模式>.yaml 重新定位 + WP="${HE_DIR}/poses/${_arm_id}_${HANDEYE_MODE}.yaml" + RES="${HE_DIR}/results/${_arm_id}_${HANDEYE_MODE}.yaml" + echo "[handeye] --mode ${HANDEYE_MODE} → poses: ${WP}" + echo "[handeye] result: ${RES}" + fi +fi + _kill_servo() { pkill -f 'visual_servo.launch' 2>/dev/null || true pkill -f 'lib/arm_application/servo_node' 2>/dev/null || true @@ -139,7 +188,7 @@ cmd_calibrate() { local ph="${SITE_HANDEYE_PIN_HOLDS:-2}" local ps="${SITE_HANDEYE_PIN_SEC:-0.45}" local skip="${SITE_HANDEYE_SKIP_UNSTABLE:-true}" - echo "=== calibrate mode=${SITE_HANDEYE_MODE:-eye_in_hand} ===" + echo "=== calibrate mode=${HANDEYE_MODE} ===" echo " board ${cols}x${rows} inner, square=${sq} m (from site.yaml)" echo " motion v=${vel} T≥${mt}s settle=${st}s qd≤${qd} pin=${ph}x${ps}s" echo " poses=${WP}" @@ -151,7 +200,7 @@ cmd_calibrate() { -p camera_source:="${CAM}" \ -p output_dir:="${WORKDIR}" \ -p result_file:="${RES}" \ - -p mode:="${SITE_HANDEYE_MODE:-eye_in_hand}" \ + -p mode:="${HANDEYE_MODE}" \ -p board_cols:="${cols}" \ -p board_rows:="${rows}" \ -p square_size_m:="${sq}" \ @@ -187,11 +236,13 @@ cmd_waypoints() { cmd_teach() { local backend="rdk" local append=false + local points=0 while [[ $# -gt 0 ]]; do case "$1" in --rdk) backend="rdk"; shift ;; --ros|--pendant) backend="ros"; shift ;; --append|--keep) append=true; shift ;; + --points) points="${2:?--points 需要一个数字}"; shift 2 ;; *) echo "Unknown teach arg: $1" >&2; exit 1 ;; esac done @@ -221,7 +272,12 @@ cmd_teach() { fi echo "=== interactive teach backend=${backend} → ${WORKDIR} ===" echo " 窗口按键: d=拖拽开 s=拖拽关 Enter=拍照记点 q=结束" - echo " 建议 8–12 个姿态，棋盘占画面约 30–70%" + if [[ "${points}" -gt 0 ]] 2>/dev/null; then + echo " 目标 ${points} 个点（采够会有提示，仍可继续多采）" + else + echo " 建议 8–12 个姿态，棋盘占画面约 30–70%" + echo " （也可用 --points N 设定目标点数）" + fi echo " 结束后: bash ~/ros2/scripts/run_handeye.sh merge" echo " bash ~/ros2/scripts/run_handeye.sh calibrate" TEACH_ARGS=( @@ -230,6 +286,7 @@ cmd_teach() { -p joint_prefix:="${PREFIX}" -p camera_source:="${CAM}" -p output_dir:="${WORKDIR}" + -p target_points:="${points}" ) if [[ -n "${SITE_CAM_SERIAL:-}" ]]; then TEACH_ARGS+=(-p camera_serial:="${SITE_CAM_SERIAL}") diff --git a/src/arm_application/arm_application/handeye_teach.py b/src/arm_application/arm_application/handeye_teach.py index 87f4395..f75c1e4 100644 --- a/src/arm_application/arm_application/handeye_teach.py +++ b/src/arm_application/arm_application/handeye_teach.py @@ -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