arm_dev
dfc945b7e0
新增
----
- arm_application/gauge_assembly_node.py：主流程。每个阶段一个 _step_*，
返回值就是状态机的事件；纯逻辑（状态机/配置/限位/夹爪后端/自检）全部
在 gauge_*.py 里，节点只做"发指令 + 读反馈 + 报错"。
* S00 自检报告带"缺了会怎样"，必检拦住、选检告警；
* S01 回 home → 切工具系 → 逆时针预绕，**事后用 TF 实测绕工具 Z 转了多少度**
（转轴偏差 >5° 直接中止：以"度"为单位的动作全建立在这条假设上）；
* S03 手外粗定位：没有 eye-to-hand 标定文件时明确报"缺哪个文件、怎么生成"，
回 S04 兜底（不做假装成功的反投影）；
* S05 稳定判据用"到均值的最远距离"，不用相邻帧之差；
* S07 悬停处测基线 → 步进下压 → 两路切向任一突变即停，法向超安全上限抬起中止；
* S08 棘轮四步（慢转→松爪→快回转→夹紧），停止判据「切向 >阈值 且 法向突增」，
掉件走 E1 重放；S09 反向拧出（比拧入多一轮）；
* E1 重放等 /gauge/reload_ok 人工确认（超时按中止），E2 任何中止都先退安全高度。
- launch/gauge_assembly.launch.py：只做一件事 —— 发布手眼静态 TF（S05 的前提），
其余判据全交给 S00（判断标准只有一处）。
- scripts/run_gauge_assembly.sh：--arm/--sim/--dry-run/--holes/--backend。
- gauge_assembly 入口（setup.py）+ 站点参数 reload_timeout_s。
- test/test_gauge_step_events.py：**读节点源码 AST**，把每个 _step_* 可能返回的
事件逐个问状态机"合法吗" —— 节点与转移表是两份数据，漏登记要在 pytest 里炸，
而不是在真机上跑到那一步才炸。
顺手修掉的两个真问题
--------------------
1. 四份 launch 里的四元数换算**已经互相抄歪了**（退化分支的公式各不相同）。
统一收口到 handeye_lib.read_transform_yaml，并加回环测试（真实标定文件 +
180°/270° 等分支切换角度）；实测 4 个 launch 输出与库完全一致。
2. pipeline_node.AssemblyPipeline 支持 node_name 复用（本流程继承它的一阶段
跟踪/二阶段定位链，但在图上必须是另一个节点名）。
验证（都在本机真跑过）
----------------------
pytest（仿真环境） → 349 passed, 14 skipped（缺 onnx 的用例跳过）
干跑全流程 → S00→S01→…→S11→DONE，exit 0
仿真里不干跑（缺 /home 等） → S00 拦住，exit 1，未发任何运动指令
另：仿真环境没有 onnx（只有真机 conda 有），相关用例改为 skip —— 让测试结果
"红就是真红"。
Co-authored-by: Cursor <cursoragent@cursor.com>
91 lines
4.0 KiB
Bash
Executable File
91 lines
4.0 KiB
Bash
Executable File
#!/usr/bin/env bash
|
||
# 通止规装配（看板 S00–S11）：自检 → 回 home+极限角 → 跟踪 → 定位 → 悬停 →
|
||
# 柔顺接触 → 棘轮拧入 → 反向拧出 → 换孔 → 复位。
|
||
#
|
||
# 前置（本脚本**不**替你起这些，只让 S00 自检告诉你缺什么）：
|
||
# T1 bash ~/ros2/scripts/run_real.sh --arm 89 # 机械臂控制器 + /home
|
||
# T2 bash ~/ros2/scripts/run_camera_d405.sh --arm 89 # 腕部 D405 预览
|
||
# T2 bash ~/ros2/scripts/run_tactile.sh # Orisys 触觉三向力
|
||
# T3 bash ~/ros2/scripts/run_gripper.sh # 夹爪服务（张开/夹紧/状态）
|
||
# —— 手眼标定产物 handeye/results/<SN>_eye_in_hand.yaml（S05 定位的前提）
|
||
#
|
||
# 为什么不在脚本里检查/拉起这些：判断标准只能有一处。脚本里查一遍、节点里
|
||
# 再查一遍，就会出现"脚本说没问题、节点说没数据"（发布者不在的假阳性），
|
||
# 而 S00 的报告是打印给人看的，带"缺了会怎样"。
|
||
#
|
||
# 用法：
|
||
# bash ~/ros2/scripts/run_gauge_assembly.sh --arm 89 # 完整装配（2 个孔）
|
||
# bash ~/ros2/scripts/run_gauge_assembly.sh --arm 89 --holes 1 # 只装 k1
|
||
# bash ~/ros2/scripts/run_gauge_assembly.sh --arm 89 --dry-run # 只验编排/配置，不动臂
|
||
# bash ~/ros2/scripts/run_gauge_assembly.sh --sim --dry-run # 仿真环境跑干跑
|
||
# bash ~/ros2/scripts/run_gauge_assembly.sh --arm 89 --backend legacy # 强制真机夹爪后端
|
||
#
|
||
# 参数接口：config/site.yaml → gauge_assembly（力阈值/角度/轮数/超时全部可调）
|
||
# 力阈值标定：先 bash ~/ros2/scripts/run_tactile.sh --cli 看读数量级
|
||
# 掉件重放：bash ~/ros2/scripts/run_gauge_assembly.sh 之外另开一个终端
|
||
# ros2 topic pub -1 /gauge/reload_ok std_msgs/Bool '{data: true}'
|
||
set -eo pipefail
|
||
ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)"
|
||
|
||
USE_SIM=false
|
||
DRY_RUN=false
|
||
HOLES=""
|
||
BACKEND="auto"
|
||
|
||
# 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
|
||
--sim) USE_SIM=true; shift ;;
|
||
--dry-run) DRY_RUN=true; shift ;;
|
||
--holes) HOLES="${2:?}"; shift 2 ;;
|
||
--backend) BACKEND="${2:?}"; shift 2 ;;
|
||
-h|--help) sed -n '2,26p' "$0"; exit 0 ;;
|
||
*) echo "Unknown: $1" >&2; exit 1 ;;
|
||
esac
|
||
done
|
||
|
||
case "${BACKEND}" in
|
||
auto|contract|legacy) ;;
|
||
*) echo "--backend 只能是 auto|contract|legacy，收到 ${BACKEND}" >&2; exit 1 ;;
|
||
esac
|
||
|
||
if [[ "${USE_SIM}" == true ]]; then
|
||
echo "=== 仿真环境（install_sim + ROS_DOMAIN_ID=77） ==="
|
||
# shellcheck disable=SC1091
|
||
source "${ROOT}/scripts/sim_env.sh"
|
||
else
|
||
# shellcheck disable=SC1091
|
||
source "${ROOT}/scripts/env.sh"
|
||
fi
|
||
|
||
if [[ -z "${SITE_ROBOT_SN:-}" ]]; then
|
||
echo "ERROR: 没选出机械臂。用 --arm 89 指定，或填 config/site.yaml 的 robot.sn" >&2
|
||
exit 1
|
||
fi
|
||
|
||
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 "=== 通止规装配 arm=${SITE_ROBOT_ARM:-?} ${SITE_ROBOT_LABEL:-} (${SITE_ROBOT_SN}) ==="
|
||
echo " 孔数=${HOLES:-site} 夹爪后端=${BACKEND} 干跑=${DRY_RUN}"
|
||
|
||
# 清掉上一轮可能残留的装配节点（同一时间只允许一个进程在指挥这条臂）
|
||
# 不 pkill 整个 arm_application：会连相机/触觉服务一起带走（那些是本脚本的前置）
|
||
pkill -f 'lib/arm_application/gauge_assembly' 2>/dev/null || true
|
||
sleep 0.5
|
||
|
||
ARGS=(gripper_backend:="${BACKEND}")
|
||
if [[ -n "${HOLES}" ]]; then ARGS+=(holes_total:="${HOLES}"); fi
|
||
if [[ "${DRY_RUN}" == true ]]; then ARGS+=(dry_run:=true); fi
|
||
|
||
echo "=== launch gauge_assembly（自检不过会自动报缺什么） ==="
|
||
# 不用 exec：exec 之后 Ctrl-C 直接打给 ros2 launch，这里的清理就没了；
|
||
# gauge_assembly 自己也会在收到 KeyboardInterrupt 时退到安全高度。
|
||
ros2 launch arm_application gauge_assembly.launch.py "${ARGS[@]}"
|