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