把当前控制栈收成一份可克隆的源码：腕部看不见时用顶视一次定到基座，二阶段对孔和手外标定跟着进同一份树。
现场 site.yaml、权重和编译产物仍不入库；模板只留占位符。 Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
arm_dev
+95
-32
@@ -68,25 +68,21 @@ if [[ ! -d "${APP_CFG}" ]]; then
|
||||
fi
|
||||
|
||||
# 手眼标定相关文件现在集中在仓库根的 handeye/ 下（见 handeye/README.md）。
|
||||
# 这些值全部由 site.yaml → export_site_env.py 提供，**包括文件名里的臂号**
|
||||
# （臂号 = 该臂 SN 末两位，由 arm_short_id() 现算）。
|
||||
# 这些值全部由 site.yaml → export_site_env.py 提供，**包括文件名里的臂号与相机名**
|
||||
# （臂号 = 该臂 SN 末两位，由 arm_short_id() 现算；相机名来自 cameras 注册表）。
|
||||
#
|
||||
# 这里刻意**不**自己拼 `<臂号>_<模式>.yaml`：`_arm_id` 和 `HANDEYE_MODE`
|
||||
# 这里刻意**不**自己拼 `<臂号>_<模式>.yaml`：`_arm_id`、`HANDEYE_MODE` 与相机名
|
||||
# 在下面几行之后才算得出来，在这里拼会拼出 `results/_.yaml` 这种空文件名，
|
||||
# 而症状是"标定写完了、用的时候读不到"。
|
||||
HE_DIR="${SITE_HANDEYE_DIR:-${ROOT}/handeye}"
|
||||
WP="${SITE_HANDEYE_WAYPOINTS:?site.yaml 没有给出该臂的手眼示教点文件}"
|
||||
CAL="${SITE_HANDEYE_CALIBRATE:-${HE_DIR}/calibrate.yaml}"
|
||||
TOUR="${SITE_HANDEYE_TOUR:-${HE_DIR}/waypoints.yaml}"
|
||||
RES="${SITE_HANDEYE_FILE:?site.yaml 没有给出该臂的手眼结果文件}"
|
||||
WORKDIR="${SITE_HANDEYE_WORKDIR:-${HE_DIR}/tmp}"
|
||||
TAUGHT="${WORKDIR}/taught_poses.txt"
|
||||
PREFIX="${SITE_JOINT_PREFIX:?SITE_JOINT_PREFIX 为空 —— site.yaml 里需要该臂的 robot.sn}"
|
||||
CAM="${SITE_CAM_NAME:-d405}"
|
||||
|
||||
# ---- --mode：切换 eye-in-hand / eye-to-hand ----
|
||||
# 必须在 WP/RES 定下来之前处理，因为它会改变读写的文件名。
|
||||
# 其余参数原样留给子命令（teach 自己有 --rdk/--ros 等）。
|
||||
# 必须在 WP/RES/板/相机定下来之前处理：模式决定了读哪个结果文件、用哪块板、
|
||||
# 开哪台相机、采点落到哪个文件。其余参数原样留给子命令（teach 自己有 --rdk 等）。
|
||||
HANDEYE_MODE="${SITE_HANDEYE_MODE:-eye_in_hand}"
|
||||
_rest=()
|
||||
while [[ $# -gt 0 ]]; do
|
||||
@@ -109,15 +105,61 @@ 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}"
|
||||
|
||||
# 取"当前模式"的那一份值。名字由 export_site_env 按模式导出成
|
||||
# SITE_HANDEYE_<字段>_EYE_IN_HAND / _EYE_TO_HAND
|
||||
# 这样"模式 → 结果文件 / 示教点 / 标定板"的对应关系只有 site.yaml 一处定义，
|
||||
# 脚本不再自己拼文件名（拼错/漏拼都是**静默**用错文件，最难查）。
|
||||
_HE_TAG="$(echo "${HANDEYE_MODE}" | tr '[:lower:]' '[:upper:]')"
|
||||
_he() {
|
||||
local name="SITE_HANDEYE_$1_${_HE_TAG}"
|
||||
printf '%s' "${!name-}"
|
||||
}
|
||||
|
||||
WP="$(_he WAYPOINTS)"
|
||||
WP="${WP:-${HE_DIR}/poses/${_arm_id:-?}_${HANDEYE_MODE}.yaml}"
|
||||
RES="$(_he FILE)"
|
||||
RES="${RES:-${HE_DIR}/results/${_arm_id:-?}_${HANDEYE_MODE}.yaml}"
|
||||
# 示教点临时文件也按模式分开：两个模式共用一份时，后一个 teach 会清掉前一个的采点，
|
||||
# 而症状只是"merge 出来的点位不对"，很难联想到是文件串了。
|
||||
TAUGHT="${WORKDIR}/taught_poses_${HANDEYE_MODE}.txt"
|
||||
|
||||
# ---- 相机：两种模式用的是**两台不同的相机** ----
|
||||
# 眼在手上 = 腕部相机（跟着臂动）；眼在手外 = 固定在工位外的相机（不随臂动）。
|
||||
# 原来这里恒用 SITE_CAM_NAME（腕部），于是 `--mode eye_to_hand` 会去开腕部相机 ——
|
||||
# 板跟着臂动、相机也跟着臂动，"标定"能跑完，但解出的是个毫无意义的错外参。
|
||||
if [[ "${HANDEYE_MODE}" == "eye_to_hand" ]]; then
|
||||
CAM="${SITE_OVERHEAD_CAM_NAME:-}"
|
||||
if [[ -z "${CAM}" ]]; then
|
||||
echo "ERROR: eye_to_hand 需要一台固定相机，但 site.yaml 的 cameras 段里没有登记的。" >&2
|
||||
echo " 加一条 role: overhead_eye_to_hand（眼在手外、不随臂动）的相机即可。" >&2
|
||||
exit 2
|
||||
fi
|
||||
CAM_SERIAL="${SITE_OVERHEAD_CAM_SERIAL:-}"
|
||||
CAM_WIDTH="${SITE_OVERHEAD_CAM_WIDTH:-}"
|
||||
CAM_HEIGHT="${SITE_OVERHEAD_CAM_HEIGHT:-}"
|
||||
CAM_FPS="${SITE_OVERHEAD_CAM_FPS:-30}"
|
||||
CAM_AE="${SITE_OVERHEAD_CAM_AUTO_EXPOSURE:-1}"
|
||||
CAM_AE_LIMIT="${SITE_OVERHEAD_CAM_AE_LIMIT_US:-8000}"
|
||||
CAM_EMITTER="${SITE_OVERHEAD_CAM_EMITTER:-1}"
|
||||
CAM_EXPOSURE="${SITE_OVERHEAD_CAM_EXPOSURE_US:-0}"
|
||||
CAM_GAIN="${SITE_OVERHEAD_CAM_GAIN:-0}"
|
||||
else
|
||||
CAM="${SITE_CAM_NAME:-d405}"
|
||||
CAM_SERIAL="${SITE_CAM_SERIAL:-}"
|
||||
CAM_WIDTH="${SITE_CAM_WIDTH:-}"
|
||||
CAM_HEIGHT="${SITE_CAM_HEIGHT:-}"
|
||||
CAM_FPS="${SITE_CAM_FPS:-30}"
|
||||
CAM_AE="${SITE_CAM_AUTO_EXPOSURE:-1}"
|
||||
CAM_AE_LIMIT="${SITE_CAM_AE_LIMIT_US:-1800}"
|
||||
CAM_EMITTER="${SITE_CAM_EMITTER:-0}"
|
||||
CAM_EXPOSURE="${SITE_CAM_EXPOSURE_US:-0}"
|
||||
CAM_GAIN="${SITE_CAM_GAIN:-0}"
|
||||
fi
|
||||
echo "[handeye] mode=${HANDEYE_MODE} poses=${WP}"
|
||||
echo "[handeye] result=${RES}"
|
||||
echo "[handeye] camera=${CAM} serial=${CAM_SERIAL:-（空）}"
|
||||
|
||||
|
||||
_kill_servo() {
|
||||
pkill -f 'visual_servo.launch' 2>/dev/null || true
|
||||
@@ -159,7 +201,9 @@ cmd_show() {
|
||||
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"
|
||||
echo " camera : ${CAM} serial=${CAM_SERIAL:-（空）}"
|
||||
echo " board : $(_he BOARD_COLS)x$(_he BOARD_ROWS) inner, square=$(_he SQUARE) m (来源=$(_he BOARD_SRC))"
|
||||
echo " （另一种模式的板见下面 export 的 标定板[...] 两行）"
|
||||
python3 "${ROOT}/scripts/lib/export_site_env.py" --print || true
|
||||
if [[ -f "${WP}" ]]; then
|
||||
python3 - <<PY
|
||||
@@ -181,15 +225,26 @@ cmd_merge() {
|
||||
}
|
||||
|
||||
cmd_calibrate() {
|
||||
# 先查配置（板尺寸）再看硬件：配置没配好的话，连硬件都不该去动。
|
||||
local cols="$(_he BOARD_COLS)"
|
||||
local rows="$(_he BOARD_ROWS)"
|
||||
local sq="$(_he SQUARE)"
|
||||
local src="$(_he BOARD_SRC)"
|
||||
# 板尺寸必须**真的配过**。没配就开标定 = 赌一个方格边长，而错了不会报错，
|
||||
# 只会让结果系统性偏（见 handeye/README.md 第 3 节）。所以这里直接拦下。
|
||||
if [[ -z "${src}" || "${src}" == "default" ]]; then
|
||||
echo "ERROR: 没有配置 [${HANDEYE_MODE}] 用的标定板尺寸。" >&2
|
||||
echo " 在 config/site.yaml 的 handeye.boards.${HANDEYE_MODE} 里写：" >&2
|
||||
echo " cols / rows / square_size_m" >&2
|
||||
echo " （内角点 = 方格数 − 1；测一下实际方格边长，别照抄另一块板。）" >&2
|
||||
exit 2
|
||||
fi
|
||||
_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}"
|
||||
@@ -200,7 +255,8 @@ cmd_calibrate() {
|
||||
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 " board ${cols}x${rows} inner, square=${sq} m (site.yaml handeye.boards.${HANDEYE_MODE}，来源=${src})"
|
||||
echo " camera ${CAM} serial=${CAM_SERIAL:-（空）}"
|
||||
echo " motion v=${vel} T≥${mt}s settle=${st}s qd≤${qd} pin=${ph}x${ps}s"
|
||||
echo " poses=${WP}"
|
||||
echo " result → ${RES}"
|
||||
@@ -209,6 +265,7 @@ cmd_calibrate() {
|
||||
--params-file "${WP}" \
|
||||
-p joint_prefix:="${PREFIX}" \
|
||||
-p camera_source:="${CAM}" \
|
||||
${CAM_SERIAL:+-p camera_serial:="${CAM_SERIAL}"} \
|
||||
-p output_dir:="${WORKDIR}" \
|
||||
-p result_file:="${RES}" \
|
||||
-p mode:="${HANDEYE_MODE}" \
|
||||
@@ -304,18 +361,18 @@ cmd_teach() {
|
||||
-p output_dir:="${WORKDIR}"
|
||||
-p target_points:="${points}"
|
||||
)
|
||||
if [[ -n "${SITE_CAM_SERIAL:-}" ]]; then
|
||||
TEACH_ARGS+=(-p camera_serial:="${SITE_CAM_SERIAL}")
|
||||
if [[ -n "${CAM_SERIAL}" ]]; then
|
||||
TEACH_ARGS+=(-p camera_serial:="${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}")
|
||||
if [[ -n "${CAM_WIDTH}" ]]; then
|
||||
TEACH_ARGS+=(-p width:="${CAM_WIDTH}" -p height:="${CAM_HEIGHT:-480}" -p fps:="${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}"
|
||||
-p auto_exposure:="$([ "${CAM_AE:-1}" = "1" ] && echo true || echo false)"
|
||||
-p ae_limit_us:="${CAM_AE_LIMIT:-1800}"
|
||||
-p emitter_enabled:="$([ "${CAM_EMITTER:-0}" = "1" ] && echo true || echo false)"
|
||||
-p exposure_us:="${CAM_EXPOSURE:-0}"
|
||||
-p gain:="${CAM_GAIN:-0}"
|
||||
)
|
||||
ros2 run arm_application handeye_teach --ros-args "${TEACH_ARGS[@]}" || true
|
||||
|
||||
@@ -362,8 +419,14 @@ cmd_teach() {
|
||||
# 但 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 &
|
||||
if [[ "${HANDEYE_MODE}" == "eye_to_hand" ]]; then
|
||||
# 眼在手外用的是固定相机（D435），不是腕部 D405 —— 拉错相机的话，
|
||||
# 标定会去订一个没人发布的图，表现是"相机没数据"。
|
||||
_cam_cmd=(bash "${ROOT}/scripts/run_camera_d435.sh" --no-preview)
|
||||
else
|
||||
_cam_cmd=(bash "${ROOT}/scripts/run_camera_d405.sh" --arm "${SITE_ROBOT_ARM:-89}" --no-preview)
|
||||
fi
|
||||
nohup "${_cam_cmd[@]}" > /tmp/run_handeye_camera.log 2>&1 &
|
||||
echo " pid=$! log=/tmp/run_handeye_camera.log"
|
||||
# _need_camera 内部失败时会自己打印原因并 exit，不需要在这里再判一次。
|
||||
_need_camera
|
||||
|
||||
Reference in New Issue
Block a user