arm_dev
2b77ec127d
痛点：TensorRT 10.13 的 Python API 没有元数据读写，engine 一转就把类名丢了；
推理侧只能退化成 str(cid)（"0"、"1"），界面上看不出是什么类，属于静默失效。
同时旧的 export_yolo_trt.sh 只能手工搬文件、手工改 site.yaml、标签无处安放。
标签子系统（model_labels.py）
- 旁挂标签文件 <模型名>.labels.yaml，放在权重旁边，文本、可手改、进 git
（二进制权重不进 git，标签是配置所以进）
- 读取优先级：旁挂文件 > ONNX 元数据 > 模型元数据 > 占位符 class_N
占位符是故意的：缺标签时要一眼看出来，而不是悄悄退回 cid
- 解析器容忍多种写法：dict / list / 逗号串 / 逐行文本 / Python repr 串
布局与转换（weights_layout.py + weights.py + weights.sh）
- models/<分组>/<模型名>.{pt,onnx,engine} + 同名 .labels.yaml
- convert：PT→ONNX→TensorRT，支持自定义名字、自动归位、固化标签
- staging/：非标准格式先进这里，转换后源文件归档到 staging/done/
- list / labels / doctor / show 四个子命令
- doctor --check-engines 能查出 engine 与 TensorRT 版本不匹配（反序列化返回空）
推理侧接入
- yolo_detector / onnx_workpiece / hole_detect_hough 统一走 read_labels()
- 画框标签不再硬编码 "gongjian"，改用实际类名
顺手修掉的真实问题
- setup.py 撞上断掉的符号链接会让整包构建失败，报错还指向 build/ 里的幽灵文件
（源文件被挪走后 build/ 里留下的断链被 os.walk 列出来）。加 _existing() 过滤。
- P3 遗漏：site.yaml / export_site_env.py / run_handeye.sh / handeye_teach.py /
handeye_calibrate.py 里的 handeye 工作目录仍是 /tmp/handeye_calib*，
与已迁到 handeye/ 的其余路径不一致。统一到 handeye/tmp/<臂号>。
- site.yaml 的 hole_weights / hole_detect.weights 原指向不存在的 best.pt，
改为实际存在的 detect_best_1.onnx；active.pt 符号链接已废弃并清理。
- weights.sh labels 原先不接受 "分组/模型名" 写法，而 list 输出正是这个形式，
照着敲会报"找不到模型"。补上，并让报错直接给出可复制的全名。
测试：225 项通过（新增 test_model_labels.py / test_weights_layout.py）。
真实 ONNX→engine 端到端转换已验过。
Co-authored-by: Cursor <cursoragent@cursor.com>
381 lines
14 KiB
Bash
Executable File
381 lines
14 KiB
Bash
Executable File
#!/usr/bin/env bash
|
||
# Hand-eye calibration helpers.
|
||
# Scripts: ~/ros2/scripts/run_handeye.sh
|
||
# 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 --ros --auto-solve
|
||
# # 示教器模式 + 采完自动 merge 并直接标定（一条命令跑完）
|
||
# 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 show # 看当前配置与点数
|
||
#
|
||
# 关于 --auto-solve（采完自动标定）的重要限制:
|
||
# 只有 --ros（示教器）模式能真正一条命令跑完。因为标定必须在 run_real.sh 的
|
||
# 控制器上走点位，而 --rdk 软拖拽要 RDK 独占，两者互斥 —— 这是硬件约束。
|
||
# --rdk --auto-solve 会照常自动 merge，然后明确告诉你接下来那两条命令。
|
||
#
|
||
# 全局选项（放在子命令前或后都可以）:
|
||
# --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 完全分开，禁止混用。
|
||
|
||
set -eo pipefail
|
||
ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)"
|
||
|
||
# shellcheck disable=SC1091
|
||
source "${ROOT}/scripts/lib/parse_arm_args.sh"
|
||
parse_arm_args "$@"
|
||
set -- "${PARSE_ARM_REST[@]}"
|
||
CMD="${1:-}"
|
||
shift || true
|
||
|
||
usage() {
|
||
sed -n '2,40p' "$0"
|
||
}
|
||
|
||
# shellcheck disable=SC1091
|
||
source "${ROOT}/scripts/env.sh"
|
||
# shellcheck disable=SC1091
|
||
source "${ROOT}/scripts/lib/load_site_config.sh"
|
||
|
||
# Prefer package source tree (editable); fall back to install share
|
||
APP_CFG="${ROOT}/src/arm_application/config"
|
||
if [[ ! -d "${APP_CFG}" ]]; then
|
||
APP_CFG="$(ros2 pkg prefix arm_application 2>/dev/null)/share/arm_application/config" || true
|
||
fi
|
||
|
||
# 手眼标定相关文件现在集中在仓库根的 handeye/ 下（见 handeye/README.md）。
|
||
# 这些 SHE 值正常情况下由 site.yaml → export_site_env.py 提供；
|
||
# 下面只是 shell 侧的兜底，路径必须与 export_site_env.py 的默认值一致。
|
||
HE_DIR="${SITE_HANDEYE_DIR:-${ROOT}/handeye}"
|
||
WP="${SITE_HANDEYE_WAYPOINTS:-${HE_DIR}/poses/89_eye_in_hand.yaml}"
|
||
CAL="${SITE_HANDEYE_CALIBRATE:-${HE_DIR}/calibrate.yaml}"
|
||
TOUR="${SITE_HANDEYE_TOUR:-${HE_DIR}/waypoints.yaml}"
|
||
RES="${SITE_HANDEYE_FILE:-${HE_DIR}/results/89_eye_in_hand.yaml}"
|
||
WORKDIR="${SITE_HANDEYE_WORKDIR:-${HE_DIR}/tmp}"
|
||
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
|
||
pkill -f 'lib/arm_application/perception_node' 2>/dev/null || true
|
||
sleep 1
|
||
}
|
||
|
||
_need_files() {
|
||
local f
|
||
for f in "$@"; do
|
||
if [[ ! -f "$f" ]]; then
|
||
echo "ERROR: missing $f" >&2
|
||
exit 2
|
||
fi
|
||
done
|
||
}
|
||
|
||
_need_robot() {
|
||
if ! ros2 service list 2>/dev/null | grep -q '/set_named_pose'; then
|
||
echo "ERROR: start run_real.sh first" >&2
|
||
exit 1
|
||
fi
|
||
}
|
||
|
||
_need_camera() {
|
||
if ! ros2 topic list 2>/dev/null | grep -q "/${CAM}/color/image_raw"; then
|
||
echo "ERROR: no /${CAM}/color/image_raw — start run_camera_d405.sh" >&2
|
||
exit 1
|
||
fi
|
||
}
|
||
|
||
cmd_show() {
|
||
echo "=== hand-eye (package + site) ==="
|
||
echo " arm : ${SITE_ROBOT_SN:-} ${SITE_ROBOT_LABEL:-}"
|
||
echo " package cfg : ${APP_CFG}"
|
||
echo " calibrate : ${CAL}"
|
||
echo " poses : ${WP}"
|
||
echo " tour : ${TOUR}"
|
||
echo " result : ${RES}"
|
||
echo " workdir : ${WORKDIR}"
|
||
echo " board : ${SITE_HANDEYE_BOARD_COLS:-?}x${SITE_HANDEYE_BOARD_ROWS:-?} inner, square=${SITE_HANDEYE_SQUARE_M:-?} m"
|
||
python3 "${ROOT}/scripts/lib/export_site_env.py" --print || true
|
||
if [[ -f "${WP}" ]]; then
|
||
python3 - <<PY
|
||
import re
|
||
from pathlib import Path
|
||
t = Path("${WP}").read_text(encoding="utf-8")
|
||
m = re.search(r"num_poses:\s*(\d+)", t)
|
||
n = int(m.group(1)) if m else "?"
|
||
print(f" num_poses in poses yaml: {n}")
|
||
PY
|
||
fi
|
||
}
|
||
|
||
cmd_merge() {
|
||
local src="${1:-${TAUGHT}}"
|
||
_need_files "${src}" "${WP}"
|
||
python3 "${ROOT}/scripts/merge_handeye_poses.py" "${src}" "${WP}"
|
||
echo "=== next: bash ~/ros2/scripts/run_handeye.sh calibrate ==="
|
||
}
|
||
|
||
cmd_calibrate() {
|
||
_need_files "${CAL}" "${WP}"
|
||
_need_robot
|
||
_need_camera
|
||
echo "=== kill visual_servo (avoid traj fight) ==="
|
||
_kill_servo
|
||
mkdir -p "${WORKDIR}"
|
||
local cols="${SITE_HANDEYE_BOARD_COLS:-8}"
|
||
local rows="${SITE_HANDEYE_BOARD_ROWS:-11}"
|
||
local sq="${SITE_HANDEYE_SQUARE_M:-0.015}"
|
||
local vel="${SITE_HANDEYE_VEL_SCALE:-0.05}"
|
||
local mt="${SITE_HANDEYE_MOVE_T:-8.0}"
|
||
local st="${SITE_HANDEYE_SETTLE_SEC:-1.5}"
|
||
local qd="${SITE_HANDEYE_SETTLE_QD:-0.015}"
|
||
local ss="${SITE_HANDEYE_SETTLE_STABLE:-0.6}"
|
||
local sto="${SITE_HANDEYE_SETTLE_TO:-8.0}"
|
||
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=${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}"
|
||
echo " result → ${RES}"
|
||
exec ros2 run arm_application handeye_calibrate --ros-args \
|
||
--params-file "${CAL}" \
|
||
--params-file "${WP}" \
|
||
-p joint_prefix:="${PREFIX}" \
|
||
-p camera_source:="${CAM}" \
|
||
-p output_dir:="${WORKDIR}" \
|
||
-p result_file:="${RES}" \
|
||
-p mode:="${HANDEYE_MODE}" \
|
||
-p board_cols:="${cols}" \
|
||
-p board_rows:="${rows}" \
|
||
-p square_size_m:="${sq}" \
|
||
-p velocity_scale:="${vel}" \
|
||
-p move_time_sec:="${mt}" \
|
||
-p settle_sec:="${st}" \
|
||
-p settle_qd_max:="${qd}" \
|
||
-p settle_stable_sec:="${ss}" \
|
||
-p settle_timeout_sec:="${sto}" \
|
||
-p pin_holds:="${ph}" \
|
||
-p pin_hold_sec:="${ps}" \
|
||
-p skip_unstable_poses:="${skip}"
|
||
}
|
||
|
||
cmd_waypoints() {
|
||
_need_files "${TOUR}" "${WP}"
|
||
_need_robot
|
||
_need_camera
|
||
echo "=== kill visual_servo ==="
|
||
_kill_servo
|
||
mkdir -p "${WORKDIR}"
|
||
echo "=== waypoint tour (Enter=confirm, q=quit) ==="
|
||
echo " saves confirmed joints → ${TAUGHT}"
|
||
echo " then: bash ~/ros2/scripts/run_handeye.sh merge"
|
||
exec ros2 run arm_application handeye_waypoints --ros-args \
|
||
--params-file "${TOUR}" \
|
||
--params-file "${WP}" \
|
||
-p joint_prefix:="${PREFIX}" \
|
||
-p camera_source:="${CAM}" \
|
||
-p append_poses_file:="${TAUGHT}"
|
||
}
|
||
|
||
cmd_teach() {
|
||
local backend="rdk"
|
||
local append=false
|
||
local points=0
|
||
local auto_solve=false
|
||
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 ;;
|
||
--auto-solve) auto_solve=true; shift ;;
|
||
*) echo "Unknown teach arg: $1" >&2; exit 1 ;;
|
||
esac
|
||
done
|
||
|
||
echo "=== kill visual_servo + camera (D405 exclusive) ==="
|
||
_kill_servo
|
||
pkill -f 'run_camera_d405' 2>/dev/null || true
|
||
pkill -f 'realsense_color_pub' 2>/dev/null || true
|
||
pkill -f 'realsense2_camera_node' 2>/dev/null || true
|
||
sleep 1
|
||
|
||
if [[ "${backend}" == "rdk" ]]; then
|
||
if ros2 service list 2>/dev/null | grep -q '/set_named_pose'; then
|
||
echo "ERROR: RDK 拖拽与 run_real.sh 互斥。请先在那个终端 Ctrl-C 停掉 run_real.sh" >&2
|
||
exit 1
|
||
fi
|
||
else
|
||
_need_robot
|
||
fi
|
||
|
||
mkdir -p "${WORKDIR}/images"
|
||
if [[ "${append}" == true ]]; then
|
||
echo "=== append: keep existing ${TAUGHT} ==="
|
||
else
|
||
# 重新设点：丢掉上次 taught，避免 merge 混进旧位姿
|
||
: > "${TAUGHT}"
|
||
fi
|
||
echo "=== interactive teach backend=${backend} → ${WORKDIR} ==="
|
||
echo " 窗口按键: d=拖拽开 s=拖拽关 Enter=拍照记点 q=结束"
|
||
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"
|
||
if [[ "${auto_solve}" == true ]]; then
|
||
echo " --auto-solve: 采完会自动 merge，并视后端决定是否直接标定"
|
||
fi
|
||
TEACH_ARGS=(
|
||
-p backend:="${backend}"
|
||
-p robot_sn:="${SITE_ROBOT_SN:-Rizon4-063989}"
|
||
-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}")
|
||
fi
|
||
if [[ -n "${SITE_CAM_WIDTH:-}" ]]; then
|
||
TEACH_ARGS+=(-p width:="${SITE_CAM_WIDTH}" -p height:="${SITE_CAM_HEIGHT:-480}" -p fps:="${SITE_CAM_FPS:-30}")
|
||
fi
|
||
TEACH_ARGS+=(
|
||
-p auto_exposure:="$([ "${SITE_CAM_AUTO_EXPOSURE:-1}" = "1" ] && echo true || echo false)"
|
||
-p ae_limit_us:="${SITE_CAM_AE_LIMIT_US:-1800}"
|
||
-p emitter_enabled:="$([ "${SITE_CAM_EMITTER:-0}" = "1" ] && echo true || echo false)"
|
||
-p exposure_us:="${SITE_CAM_EXPOSURE_US:-0}"
|
||
-p gain:="${SITE_CAM_GAIN:-0}"
|
||
)
|
||
ros2 run arm_application handeye_teach --ros-args "${TEACH_ARGS[@]}" || true
|
||
|
||
# ---- 采点结束 ----
|
||
if [[ "${auto_solve}" != true ]]; then
|
||
echo
|
||
echo "采点结束。接着跑（或下次加 --auto-solve 让它自动接）："
|
||
echo " bash ~/ros2/scripts/run_handeye.sh merge --arm ${SITE_ROBOT_ARM:-89}"
|
||
echo " bash ~/ros2/scripts/run_handeye.sh calibrate --arm ${SITE_ROBOT_ARM:-89}"
|
||
return 0
|
||
fi
|
||
|
||
echo
|
||
echo "=== --auto-solve: 自动 merge ==="
|
||
if [[ ! -s "${TAUGHT}" ]]; then
|
||
echo "ERROR: 没采到任何点（${TAUGHT} 为空），中止。" >&2
|
||
return 1
|
||
fi
|
||
if ! python3 "${ROOT}/scripts/merge_handeye_poses.py" "${TAUGHT}" "${WP}"; then
|
||
echo "ERROR: merge 失败，点还在 ${TAUGHT}，可手动重跑 merge。" >&2
|
||
return 1
|
||
fi
|
||
echo " 已写入 ${WP}"
|
||
|
||
# RDK 软拖拽与控制器互斥：软拖拽要 RDK 独占，标定必须在 run_real.sh 的
|
||
# 控制器上走点位，两者无法同时存在。这是硬件约束，脚本绕不过去，
|
||
# 所以这里如实停下并给出下一步，而不是硬跑一个必然失败的 calibrate。
|
||
if [[ "${backend}" == "rdk" ]]; then
|
||
echo
|
||
echo "⚠️ 当前是 RDK 拖拽模式（--rdk），无法自动接着求解。"
|
||
echo " 原因：软拖拽需要 RDK 独占，而标定需要通过 run_real.sh 的控制器走点位，"
|
||
echo " 两者不能同时存在。"
|
||
echo
|
||
echo " 继续两步即可："
|
||
echo " 1) 另开终端: bash ~/ros2/scripts/run_real.sh --arm ${SITE_ROBOT_ARM:-89}"
|
||
echo " 2) 回到这里: bash ~/ros2/scripts/run_handeye.sh calibrate --arm ${SITE_ROBOT_ARM:-89}"
|
||
echo
|
||
echo " 想真正一条命令跑完，请改用示教器 Free Drive 模式："
|
||
echo " bash ~/ros2/scripts/run_handeye.sh teach --ros --auto-solve"
|
||
return 0
|
||
fi
|
||
|
||
# --ros（示教器）后端：控制器本来就在跑，可以无缝接上标定。
|
||
# 但 teach 阶段把相机杀掉了（USB 独占），标定拍照要用，得先拉回来。
|
||
echo
|
||
echo "=== 重启相机（teach 阶段杀掉了它，标定拍照需要）==="
|
||
nohup bash "${ROOT}/scripts/run_camera_d405.sh" --arm "${SITE_ROBOT_ARM:-89}" \
|
||
--no-preview > /tmp/run_handeye_camera.log 2>&1 &
|
||
echo " pid=$! log=/tmp/run_handeye_camera.log"
|
||
# _need_camera 内部失败时会自己打印原因并 exit，不需要在这里再判一次。
|
||
_need_camera
|
||
|
||
echo
|
||
echo "=== --auto-solve: 自动 calibrate ==="
|
||
cmd_calibrate
|
||
}
|
||
|
||
case "${CMD}" in
|
||
calibrate|calib) cmd_calibrate ;;
|
||
waypoints|tour) cmd_waypoints "$@" ;;
|
||
teach) cmd_teach "$@" ;;
|
||
merge) cmd_merge "$@" ;;
|
||
show) cmd_show ;;
|
||
-h|--help|help|"") usage; [[ -n "${CMD}" ]] || exit 0; exit 0 ;;
|
||
*) echo "Unknown: ${CMD}" >&2; usage >&2; exit 1 ;;
|
||
esac
|