chore: 框架科学优化前的完整基线
在开始'设备契约层 + 目录重构 + 通止规装配'改造之前，先钉住当前可运行状态， 使后续任何删除/重构都可回退。 本基线包含： - 现有 ROS 2 工作区全部源码（arm_application / arm_control / arm_bringup 等） - 官方 flexiv_ros2 栈（含 flexiv_hardware / flexiv_controllers 等符号链接） - 设备契约层 src/arm_interfaces（新增，46 项单测通过） - devices/ 设备选型与品牌清单（新增） - scripts/gen_site_example.py 与由它生成的 config/site.example.yaml 按 .gitignore 排除：build/ / install/ / log/ / config/site.yaml / 模型权重二进制。
This commit is contained in:
arm_dev
Executable
+254
@@ -0,0 +1,254 @@
|
||||
#!/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/
|
||||
#
|
||||
# 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 --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
|
||||
#
|
||||
# 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,18p' "$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
|
||||
|
||||
WP="${SITE_HANDEYE_WAYPOINTS:-${APP_CFG}/handeye_poses.yaml}"
|
||||
CAL="${SITE_HANDEYE_CALIBRATE:-${APP_CFG}/handeye_calibrate.yaml}"
|
||||
TOUR="${SITE_HANDEYE_TOUR:-${APP_CFG}/handeye_waypoints.yaml}"
|
||||
RES="${SITE_HANDEYE_FILE:-${APP_CFG}/handeye_result.yaml}"
|
||||
WORKDIR="${SITE_HANDEYE_WORKDIR:-/tmp/handeye_calib}"
|
||||
TAUGHT="${WORKDIR}/taught_poses.txt"
|
||||
PREFIX="${SITE_JOINT_PREFIX:-Rizon4-063989_}"
|
||||
CAM="${SITE_CAM_NAME:-d405}"
|
||||
|
||||
_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:-11}"
|
||||
local rows="${SITE_HANDEYE_BOARD_ROWS:-16}"
|
||||
local sq="${SITE_HANDEYE_SQUARE_M:-0.011}"
|
||||
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=${SITE_HANDEYE_MODE:-eye_in_hand} ==="
|
||||
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:="${SITE_HANDEYE_MODE:-eye_in_hand}" \
|
||||
-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
|
||||
while [[ $# -gt 0 ]]; do
|
||||
case "$1" in
|
||||
--rdk) backend="rdk"; shift ;;
|
||||
--ros|--pendant) backend="ros"; shift ;;
|
||||
--append|--keep) append=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=结束"
|
||||
echo " 建议 8–12 个姿态，棋盘占画面约 30–70%"
|
||||
echo " 结束后: bash ~/ros2/scripts/run_handeye.sh merge"
|
||||
echo " bash ~/ros2/scripts/run_handeye.sh calibrate"
|
||||
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}"
|
||||
)
|
||||
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}"
|
||||
)
|
||||
exec ros2 run arm_application handeye_teach --ros-args "${TEACH_ARGS[@]}"
|
||||
}
|
||||
|
||||
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
|
||||
Reference in New Issue
Block a user