arm_dev
7af9e3d4b6
搜孔开局会抬到悬高再落回 hole_hover_z_m，螺纹还没咬合臂就离开孔口。没有接触传感器，到位只认 final_gap 算出的法兰高度，贴面后停在这个 Z。09:25 厂家 MoveL 切回 NRT_JOINT_POSITION 的第一拍把整段关节差算成 1.686 rad/s，前瞻再推 0.07 rad，光心从 5 mm 冲到 276 mm 后急停。切回后钉住结束瞬间的关节角并清零速度，JTC 还指着出发前的关节时不放行。 Co-authored-by: Cursor <cursoragent@cursor.com>
329 lines
15 KiB
Bash
Executable File
329 lines
15 KiB
Bash
Executable File
#!/usr/bin/env bash
|
||
# 三阶段装配流程的完整入口：一阶段跟踪 → 二阶段对孔 → 三阶段搜孔轻压。
|
||
#
|
||
# 为什么要单独有这个脚本：
|
||
# pipeline.launch.py 只管两件事 —— 发布手眼静态 TF、起 assembly_pipeline 串联节点。
|
||
# 它**不**拉相机、也**不**拉触觉（见它文件头的注释）。前置的相机与触感服务
|
||
# 必须由本脚本拉起一次，否则二/三阶段会因为拿不到图像或力数据而空转。
|
||
#
|
||
# 本脚本做的事（顺序固定）：
|
||
# 1) 确认机械臂控制器已 ACTIVE（不在就直接报错退出，不假装能跑）
|
||
# 2) 拉起 D405 相机（已在发图就跳过）
|
||
# 3) 拉起触觉服务（只有左臂装了传感器；右臂自动跳过）
|
||
# 4) exec pipeline.launch.py，把下面的 CLI 参数映射成它的 launch 参数
|
||
#
|
||
# 用法：
|
||
# bash ~/ros2/scripts/run_all.sh --arm 89 # 全流程
|
||
# bash ~/ros2/scripts/run_all.sh --arm 89 --hole k2 # 指定对 k2 孔
|
||
# bash ~/ros2/scripts/run_all.sh --arm 89 --skip-stage1 # 臂已在工件上方，跳过跟踪
|
||
# bash ~/ros2/scripts/run_all.sh --arm 89 --depth 6 --pitch 1.0
|
||
# bash ~/ros2/scripts/run_all.sh --arm 89 --camera-preview # 相机带 Qt 预览窗
|
||
# bash ~/ros2/scripts/run_all.sh --arm 89 --no-camera # 相机已在别处拉起了
|
||
# bash ~/ros2/scripts/run_all.sh --arm 89 --no-tactile # 不起触觉服务
|
||
# bash ~/ros2/scripts/run_all.sh --arm 89 --no-descent # 对准后只降到观测高度（默认离面 15mm）
|
||
# bash ~/ros2/scripts/run_all.sh --arm 89 --init-z 0.35 # 启动后先到这个法兰高度 [m]
|
||
# bash ~/ros2/scripts/run_all.sh --arm 89 --hover-z 0.34 # 对孔悬停法兰高度 [m]，不安全会拒绝
|
||
# bash ~/ros2/scripts/run_all.sh --arm 89 --final-gap 8 # 下臂锁 XY，规尖停在离面 8mm（site 默认 2）
|
||
# bash ~/ros2/scripts/run_all.sh --arm 89 --d435-only # 拍一张 K0，厂家 MoveL 把工具尖 XY 对准它（先停 run_real.sh）
|
||
#
|
||
# 前置（另开一个终端，先跑起来）：
|
||
# bash ~/ros2/scripts/run_real.sh --arm 89
|
||
#
|
||
# 注意：run_pipeline.sh 是本脚本的别名（历史名字），两者等价。
|
||
|
||
set -eo pipefail
|
||
ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)"
|
||
|
||
# ---- 默认值：全部沿用 site.yaml / launch 默认，CLI 只做覆盖 ----
|
||
SKIP_STAGE1=false
|
||
SKIP_STAGE2=false
|
||
SKIP_STAGE3=false
|
||
HOLE=""
|
||
DEPTH=""
|
||
PITCH=""
|
||
PIPELINE_PREVIEW=true # 传给 pipeline 节点：是否显示推理预览
|
||
RESNAP=false
|
||
WITH_CAMERA=true
|
||
WITH_TACTILE=true
|
||
CAMERA_PREVIEW=false
|
||
NO_DESCENT=false
|
||
D435_ONLY=false
|
||
INIT_Z=""
|
||
HOVER_Z=""
|
||
FINAL_GAP=""
|
||
|
||
# --sn/--arm 必须先解析：load_site_config.sh 靠 ROBOT_SN 决定加载哪套机器人配置
|
||
# shellcheck disable=SC1091
|
||
source "${ROOT}/scripts/lib/parse_arm_args.sh"
|
||
parse_arm_args "$@"
|
||
set -- "${PARSE_ARM_REST[@]}"
|
||
|
||
while [[ $# -gt 0 ]]; do
|
||
case "$1" in
|
||
--skip-stage1) SKIP_STAGE1=true; shift ;;
|
||
--skip-stage2) SKIP_STAGE2=true; shift ;;
|
||
--skip-stage3) SKIP_STAGE3=true; shift ;;
|
||
--hole) HOLE="${2:?--hole 需要一个孔名（k1 或 k2）}"; shift 2 ;;
|
||
--depth) DEPTH="${2:?--depth 需要毫米数}"; shift 2 ;;
|
||
--pitch) PITCH="${2:?--pitch 需要螺距毫米数}"; shift 2 ;;
|
||
--resnap) RESNAP=true; shift ;;
|
||
# 不到贴面 2mm。对准后仍降到 site 的 observe_gap_mm（默认离面 15mm），
|
||
# 方便看规尖偏没偏；对不准就下到 2mm 会把规顶在孔沿上（2026-10-08 真机 63N）。
|
||
--no-descent) NO_DESCENT=true; shift ;;
|
||
--d435-only) D435_ONLY=true; SKIP_STAGE2=true; SKIP_STAGE3=true; shift ;;
|
||
--init-z) INIT_Z="${2:?--init-z 需要法兰高度，单位米}"; shift 2 ;;
|
||
--hover-z) HOVER_Z="${2:?--hover-z 需要法兰高度，单位米}"; shift 2 ;;
|
||
--final-gap) FINAL_GAP="${2:?--final-gap 需要规尖离面间隙，单位毫米}"; shift 2 ;;
|
||
--no-preview) PIPELINE_PREVIEW=false; shift ;;
|
||
--camera-preview) CAMERA_PREVIEW=true; shift ;;
|
||
--no-camera) WITH_CAMERA=false; shift ;;
|
||
--no-tactile) WITH_TACTILE=false; shift ;;
|
||
-h|--help) sed -n '2,32p' "$0"; exit 0 ;;
|
||
*) echo "Unknown: $1" >&2; exit 1 ;;
|
||
esac
|
||
done
|
||
|
||
if [[ -n "${FINAL_GAP}" && "${NO_DESCENT}" == true ]]; then
|
||
echo "ERROR: 同时写了 --final-gap 和 --no-descent，不知道听谁的。" >&2
|
||
exit 1
|
||
fi
|
||
|
||
# shellcheck disable=SC1091
|
||
source "${ROOT}/scripts/env.sh"
|
||
# shellcheck disable=SC1091
|
||
source "${ROOT}/scripts/lib/load_site_config.sh"
|
||
|
||
if [[ -n "${SITE_FASTDDS_PROFILE:-}" && -f "${SITE_FASTDDS_PROFILE}" ]]; then
|
||
export FASTRTPS_DEFAULT_PROFILES_FILE="${SITE_FASTDDS_PROFILE}"
|
||
export RMW_IMPLEMENTATION="${RMW_IMPLEMENTATION:-rmw_fastrtps_cpp}"
|
||
fi
|
||
|
||
echo "=== run_all（三阶段串联） arm=${SITE_ROBOT_ARM:-?} ${SITE_ROBOT_LABEL:-} (${SITE_ROBOT_SN:-?}) ==="
|
||
|
||
# D435 单次定位不走关节轨迹。拍一张 → K0 用眼在手外 T 变到基座 → 厂家 MoveL。
|
||
# RDK 与 run_real.sh 互斥，所以这条路径要求控制器没开。
|
||
if [[ "${D435_ONLY}" == true ]]; then
|
||
echo "=== D435 单次定位：厂家 RDK MoveL（先停 run_real.sh）==="
|
||
if timeout 3 ros2 service list 2>/dev/null | grep -q "^/controller_manager/list_controllers$"; then
|
||
echo "ERROR: run_real.sh 还占着 RDK，MoveL 会被打回 IDLE。先在那个终端 Ctrl-C。" >&2
|
||
exit 1
|
||
fi
|
||
D435_TOPIC="/d435/color/image_raw"
|
||
_d435_ok() {
|
||
timeout 2 ros2 topic list 2>/dev/null | grep -qx "${D435_TOPIC}" || return 1
|
||
timeout 3 ros2 topic echo --once "${D435_TOPIC}" 2>/dev/null | grep -q "^header:"
|
||
}
|
||
if _d435_ok; then
|
||
echo "=== 顶视 ${D435_TOPIC} 已在发图 ==="
|
||
else
|
||
echo "=== 拉起顶视 D435 ==="
|
||
nohup bash "${ROOT}/scripts/run_camera_d435.sh" --no-preview > /tmp/run_all_d435.log 2>&1 &
|
||
echo " d435 pid=$! log=/tmp/run_all_d435.log"
|
||
d435_ok=false
|
||
for _i in $(seq 1 24); do
|
||
if _d435_ok; then d435_ok=true; break; fi
|
||
sleep 0.5
|
||
done
|
||
if [[ "${d435_ok}" != true ]]; then
|
||
echo "ERROR: ${D435_TOPIC} 仍无图像。看 /tmp/run_all_d435.log" >&2
|
||
exit 1
|
||
fi
|
||
fi
|
||
echo "=== 拍一张，K0 × 眼在手外 T → 基座 ==="
|
||
k0_line="$(python3 "${ROOT}/scripts/d435_k0_once.py")"
|
||
echo " ${k0_line}"
|
||
read -r _tag kx ky kz <<<"${k0_line}"
|
||
if [[ "${_tag}" != "K0_BASE" ]]; then
|
||
echo "ERROR: 没有得到 K0_BASE，得到：${k0_line}" >&2
|
||
exit 1
|
||
fi
|
||
echo "=== MoveL：XY 对准 K0，工具尖高度保持当前（K0 的 z 是工件表面）==="
|
||
# --go：不按直线距离拦截。800 mm 是脚本自己的保护，不是机械臂限位。
|
||
exec /home/cc/rdk_py310/bin/python "${ROOT}/scripts/rdk_goto_xyz.py" \
|
||
--xyz "${kx}" "${ky}" "${kz}" --keep-z --go --vel 0.05
|
||
fi
|
||
|
||
# ---- 1) 控制器必须在。判活方式与 run_stage2_locate.sh 保持一致 ----
|
||
echo "=== 等待 rizon_arm_controller ACTIVE（T1: run_real.sh --arm ${SITE_ROBOT_ARM:-?}）==="
|
||
# 先看服务在不在，再 call —— 顺序反过来的话，服务不存在时每次 call 都要耗满
|
||
# 超时才返回，30 次循环能白等两三分钟（实测 63s），用户会以为脚本卡死了。
|
||
ready=false
|
||
if timeout 3 ros2 service list 2>/dev/null | grep -q "^/controller_manager/list_controllers$"; then
|
||
for _i in $(seq 1 20); do
|
||
if timeout 3 ros2 service call /controller_manager/list_controllers \
|
||
controller_manager_msgs/srv/ListControllers 2>/dev/null \
|
||
| grep -q "name='rizon_arm_controller', state='active'"; then
|
||
ready=true
|
||
break
|
||
fi
|
||
sleep 0.5
|
||
done
|
||
fi
|
||
if [[ "${ready}" != true ]]; then
|
||
echo "ERROR: 机械臂控制器未激活。请先在另一终端跑：" >&2
|
||
echo " bash ~/ros2/scripts/run_real.sh --arm ${SITE_ROBOT_ARM:-89}" >&2
|
||
echo "（若该终端已在跑，先确认它没报错退出、且 Rizon 上电使能成功。）" >&2
|
||
exit 1
|
||
fi
|
||
echo " rizon_arm_controller ACTIVE"
|
||
|
||
# ---- 2) 相机。已在发图就跳过（避免重复起导致 USB 独占失败）----
|
||
CAM_NS="${SITE_CAM_NAME:-d405_left}"
|
||
IMG_TOPIC="/${CAM_NS}/color/image_raw"
|
||
|
||
_camera_ok() {
|
||
# 以真实消息为准：topic info 的 Publisher count 依赖 DDS 发现，
|
||
# 进程频繁重启后会出现假阴性（run_stage2_locate.sh 里踩过这个坑）。
|
||
# 但先查 topic 列表，避免话题根本不存在时 echo 白等到超时。
|
||
timeout 2 ros2 topic list 2>/dev/null | grep -qx "${IMG_TOPIC}" || return 1
|
||
timeout 3 ros2 topic echo --once "${IMG_TOPIC}" 2>/dev/null | grep -q "^header:"
|
||
}
|
||
|
||
if [[ "${WITH_CAMERA}" == true ]]; then
|
||
if _camera_ok; then
|
||
echo "=== 相机 ${IMG_TOPIC} 已在发图，跳过拉起 ==="
|
||
else
|
||
echo "=== 拉起相机 ${CAM_NS} ==="
|
||
# 后台拉起；默认不带预览窗（nohup 下开 Qt 窗口没有意义）。
|
||
# 想实时看画面就用 --camera-preview，或干脆另开终端跑 run_camera_d405.sh --preview。
|
||
CAM_ARGS=()
|
||
[[ "${CAMERA_PREVIEW}" == true ]] && CAM_ARGS+=(--preview) || CAM_ARGS+=(--no-preview)
|
||
nohup bash "${ROOT}/scripts/run_camera_d405.sh" --arm "${SITE_ROBOT_ARM:-89}" \
|
||
"${CAM_ARGS[@]}" > /tmp/run_all_camera.log 2>&1 &
|
||
echo " camera pid=$! log=/tmp/run_all_camera.log"
|
||
cam_ok=false
|
||
for _i in $(seq 1 24); do # 最多等 ~12s；RealSense 枚举 + 首帧比想象中慢
|
||
if _camera_ok; then cam_ok=true; break; fi
|
||
sleep 0.5
|
||
done
|
||
if [[ "${cam_ok}" != true ]]; then
|
||
echo "ERROR: ${IMG_TOPIC} 仍无图像。看日志：/tmp/run_all_camera.log" >&2
|
||
echo " 常见原因：USB 没插好；或另一个进程占着相机（USB 是独占的）。" >&2
|
||
exit 1
|
||
fi
|
||
echo " 相机 ${CAM_NS} 出图 OK"
|
||
fi
|
||
# 一阶段混合跟踪：D405 看不见时要马上拍 D435。顶视没起来就只能干等。
|
||
D435_TOPIC="/d435/color/image_raw"
|
||
_d435_ok() {
|
||
timeout 2 ros2 topic list 2>/dev/null | grep -qx "${D435_TOPIC}" || return 1
|
||
timeout 3 ros2 topic echo --once "${D435_TOPIC}" 2>/dev/null | grep -q "^header:"
|
||
}
|
||
if _d435_ok; then
|
||
echo "=== 顶视 ${D435_TOPIC} 已在发图，跳过拉起 ==="
|
||
else
|
||
echo "=== 拉起顶视 D435（一阶段一次性定位）==="
|
||
D435_ARGS=()
|
||
[[ "${CAMERA_PREVIEW}" == true ]] && D435_ARGS+=(--preview) || D435_ARGS+=(--no-preview)
|
||
nohup bash "${ROOT}/scripts/run_camera_d435.sh" \
|
||
"${D435_ARGS[@]}" > /tmp/run_all_d435.log 2>&1 &
|
||
echo " d435 pid=$! log=/tmp/run_all_d435.log"
|
||
d435_ok=false
|
||
for _i in $(seq 1 24); do
|
||
if _d435_ok; then d435_ok=true; break; fi
|
||
sleep 0.5
|
||
done
|
||
if [[ "${d435_ok}" != true ]]; then
|
||
echo "ERROR: ${D435_TOPIC} 仍无图像。看日志：/tmp/run_all_d435.log" >&2
|
||
echo " 混合跟踪没有 D435 就切不出去。USB 独占或没插好都会这样。" >&2
|
||
exit 1
|
||
fi
|
||
echo " 顶视 D435 出图 OK"
|
||
fi
|
||
else
|
||
echo "=== --no-camera：跳过相机检查 ==="
|
||
fi
|
||
|
||
# ---- 3) 触觉服务。只有左臂装了传感器；只有三阶段搜孔才用力 ----
|
||
# --skip-stage3 时如果还去等，USB 打不开会假死：话题在、connected 却是 false，
|
||
# 每次 echo 吃满超时 × 60 次 ≈ 3 分钟，终端上没有任何新行（2026-10-09 真机）。
|
||
_SKIP_TACTILE_REASON=""
|
||
if [[ "${SKIP_STAGE3}" == true && "${WITH_TACTILE}" == true ]]; then
|
||
echo "=== --skip-stage3：一/二阶段不用触觉，跳过拉起 ==="
|
||
WITH_TACTILE=false
|
||
_SKIP_TACTILE_REASON="stage3"
|
||
fi
|
||
|
||
# 同样先查 topic 列表再 echo，避免服务没起时反复撞超时。
|
||
TACTILE_TOPIC="/tactile/left/status"
|
||
_tactile_ok() {
|
||
timeout 2 ros2 topic list 2>/dev/null | grep -qx "${TACTILE_TOPIC}" || return 1
|
||
timeout 1 ros2 topic echo "${TACTILE_TOPIC}" --once 2>/dev/null | grep -q "connected: true"
|
||
}
|
||
|
||
if [[ "${WITH_TACTILE}" == true ]]; then
|
||
# 本臂有没有触觉由 site.yaml 的 robots[].tactile 说了算。
|
||
# 以前这里写的是 `SN == Rizon4-063989`：换一台机器人就再也拉不起触觉服务，
|
||
# 而且失败现象是"搜孔时力阈值全是 0"，离真正原因很远。
|
||
if [[ "${SITE_ROBOT_HAS_TACTILE:-0}" == "1" ]]; then
|
||
if _tactile_ok; then
|
||
echo "=== 触觉 ${TACTILE_TOPIC} 已在发，跳过拉起 ==="
|
||
else
|
||
echo "=== 拉起触觉服务（左臂）==="
|
||
# 先清干净：上一次异常退出留下的 worker 会占死 /dev/video*，
|
||
# 新进程会以"打不开相机"的形式失败，报错信息看不出根因。
|
||
pkill -f 'lib/arm_application/tactile' 2>/dev/null || true
|
||
pkill orisys_tac_w 2>/dev/null || true
|
||
pkill -f 'arm_application/tactile_node.py' 2>/dev/null || true
|
||
sleep 1.0
|
||
nohup ros2 launch arm_application tactile.launch.py > /tmp/run_all_tactile.log 2>&1 &
|
||
echo " tactile pid=$! log=/tmp/run_all_tactile.log"
|
||
echo " 等触觉 connected（USB 打不开会在十几秒内跳过，不会空等几分钟）"
|
||
t_ok=false
|
||
for _i in $(seq 1 8); do
|
||
if _tactile_ok; then
|
||
t_ok=true
|
||
break
|
||
fi
|
||
# 日志已经写明打不开：再给两轮自愈，然后放行。不许在这里空转到超时。
|
||
if [[ "${_i}" -ge 4 ]] && grep -q "connect failed" /tmp/run_all_tactile.log 2>/dev/null; then
|
||
echo " 触觉 USB 打不开，停止等待" >&2
|
||
break
|
||
fi
|
||
echo " 触觉还未 connected（${_i}/8）"
|
||
sleep 0.5
|
||
done
|
||
if [[ "${t_ok}" != true ]]; then
|
||
# 触觉不通不阻断主流程：三阶段的搜孔轻压需要力数据，但一/二阶段不需要。
|
||
# 让用户自己判断是先把传感器弄好，还是只跑到对孔就停。
|
||
echo "WARN: 触觉话题仍未就绪（传感器没接？）。流程继续，三阶段搜孔会受影响。" >&2
|
||
echo " 日志：/tmp/run_all_tactile.log" >&2
|
||
else
|
||
echo " 触觉就绪 OK"
|
||
fi
|
||
fi
|
||
else
|
||
echo "=== 触觉只有左臂装了；当前 ${SITE_ROBOT_ARM:-?}，跳过 ==="
|
||
fi
|
||
elif [[ "${_SKIP_TACTILE_REASON}" != "stage3" ]]; then
|
||
echo "=== --no-tactile：跳过触觉检查 ==="
|
||
fi
|
||
|
||
# ---- 4) 清掉可能打架的旧进程 ----
|
||
echo "=== 清理旧的一阶段伺服/感知进程 ==="
|
||
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
|
||
pkill -f 'lib/arm_application/assembly_pipeline' 2>/dev/null || true
|
||
sleep 1
|
||
|
||
# ---- 5) exec pipeline.launch.py（把 CLI 映射成它的 launch 参数）----
|
||
ARGS=(
|
||
"skip_stage1:=${SKIP_STAGE1}"
|
||
"skip_stage2:=${SKIP_STAGE2}"
|
||
"skip_stage3:=${SKIP_STAGE3}"
|
||
"preview:=${PIPELINE_PREVIEW}"
|
||
"resnap:=${RESNAP}"
|
||
"no_descent:=${NO_DESCENT}"
|
||
"d435_move_only:=${D435_ONLY}"
|
||
)
|
||
[[ -n "${INIT_Z}" ]] && ARGS+=("init_z_m:=${INIT_Z}")
|
||
[[ -n "${HOVER_Z}" ]] && ARGS+=("hover_z_m:=${HOVER_Z}")
|
||
[[ -n "${FINAL_GAP}" ]] && ARGS+=("final_gap_mm:=${FINAL_GAP}")
|
||
[[ -n "${HOLE}" ]] && ARGS+=("target_hole:=${HOLE}")
|
||
[[ -n "${DEPTH}" ]] && ARGS+=("insert_depth_mm:=${DEPTH}")
|
||
[[ -n "${PITCH}" ]] && ARGS+=("screw_pitch_mm:=${PITCH}")
|
||
|
||
echo "=== launch pipeline hole=${HOLE:-site} depth=${DEPTH:-site} pitch=${PITCH:-site} init_z=${INIT_Z:-site} hover_z=${HOVER_Z:-site} final_gap=${FINAL_GAP:-site} skip1=${SKIP_STAGE1} skip2=${SKIP_STAGE2} skip3=${SKIP_STAGE3} no_descent=${NO_DESCENT} d435_only=${D435_ONLY} ==="
|
||
exec ros2 launch arm_application pipeline.launch.py "${ARGS[@]}"
|