arm_dev
6922b18187
为什么
------
应用层只认 arm_msgs/srv/GripperExecute（开口毫米），真机与仿真必须同一套接口。
但 GN01 的 URDF 里 <mimic> 是**线性近似**：直接按线性系数下发 finger_width_joint，
"命令开 20 mm"在 Gazebo 里量出来是 21.75 mm —— 抓不住工件，而且看起来像算法问题。
实测（2026-09-24，Gazebo 内量两指尖间距）：
线性下发 20 mm → 实际 21.74 mm（偏差 1.74 mm，超 2 mm 抓取容差的前半步）
走 FK 反解 → 实际 20.00 mm（≤0.5 mm）
怎么改
------
- 新增 sim_gripper_kin.py：解析 URDF 正运动学，算出"主关节值 ↔ 真实开口 mm"的
单调曲线，反解用二分。曲线非单调/零行程时**主动报错**，不静默退化成线性。
- sim_gripper.py：开口换算全部走该曲线；容差统一改成毫米。
- sim_gripper_adapter.py：反馈改取**从动关节**（见下）；is_gripped 按毫米判定。
- 新增 check_sim_gripper.py：三层验收（服务/开口一致/指尖真实几何行程 + 对称性）。
踩到的坑
--------
- finger_width_joint 挂在无质量的 unused_finger_width_link 上，与手指**没有机械
连接**：命令到 0.100，它的 /joint_states 恒为 1.4e-08，6 个从动关节才准到
0.7854。反馈若取它，/gripper/status 会永远报"开口 0 mm"。
- 残留进程会伪造出"代码写错了"的现场：旧 sim_gripper_adapter 与新实例抢话题，
验收读到"指尖间距 0.03 mm"（真实 127 mm）；孤儿 static_transform_publisher
用 latched /tf_static 把旧相机外参按在新仿真上。
→ 新增 scripts/lib/sim_procs.sh；run_sim_gz.sh 用 setsid 独立进程组，
PGID 记进 log_sim/sim.pgid，stop 时整组 kill；启动前预检拒绝带病启动。
- 验收脚本自己也错过两次（把 TF 的米当毫米比 → 127 mm 显示成 0.13 mm；
用会转动指垫的中点漂移判对称 → 必然位移 16 mm）。判据改成"两侧各走多远"，
单位只在 _dist_mm 里换算一次。
验证
----
python3 -m pytest src/arm_sim_gazebo/test/ -q → 307 passed
python3 scripts/check_sim_gripper.py → 三层全过
第 2 层：请求/反馈开口最大偏差 < 0.5 mm
第 3 层：期望行程 vs 实际行程、左右行程差，均 < 1.5 mm
bash scripts/stop_sim_gz.sh --list → 整组列出（含 static tf）
文档
----
- 仿真手册：新增 §4.4（开口 mm 怎么换算）、坑 12（宽度关节不动、
反馈必须取从动关节）；停止一节改写成进程组语义。
- FRAMEWORK_RULES：新增 R4.11（残留进程会伪造现场）、R4.12（验收判据要挑
与实现无关的物理量）；R4.9 表补夹爪；清单补 13/14 两条。
- HANDBOOK：新增 §3.4「在仿真里开合夹爪」，补 §4.4 的两条新教训。
Co-authored-by: Cursor <cursoragent@cursor.com>
135 lines
5.7 KiB
Bash
Executable File
135 lines
5.7 KiB
Bash
Executable File
#!/usr/bin/env bash
|
||
# =============================================================================
|
||
# scripts/stop_sim_gz.sh —— 干净地停掉 Gazebo 仿真栈
|
||
#
|
||
# 为什么要专门写一个脚本，而不是让你敲 pkill：
|
||
#
|
||
# ⚠️ `pkill -f <模式>` 在"用脚本启动仿真"的场景下会**把自己的启动壳一起杀掉**。
|
||
# 因为那条命令自己的命令行里就含那个模式字符串，pkill -f 匹配整条命令行，
|
||
# 于是它先匹配到自己所在的 shell。表现是：命令返回得飞快、什么输出都没有、
|
||
# 仿真也没停成 —— 而这看起来像脚本写错了，极难往"进程匹配"上想。
|
||
#
|
||
# 本脚本的做法：扫描候选 PID → 排除自己/祖先/扫描自身的临时进程 → kill，
|
||
# 最后**再扫一遍复查**（杀掉后仍有残留就报出来）。
|
||
# 所以它可以在任何调用链里安全使用（包括被其它脚本 `source`/调用）。
|
||
#
|
||
# 用法：
|
||
# bash scripts/stop_sim_gz.sh # 停仿真（TERM，等 5s，仍在则 KILL）
|
||
# bash scripts/stop_sim_gz.sh --list # 只列出会被停掉的进程，不动手
|
||
# =============================================================================
|
||
set -uo pipefail
|
||
|
||
LIST_ONLY=0
|
||
[ "${1:-}" = "--list" ] && LIST_ONLY=1
|
||
|
||
# \"哪些进程算仿真栈\"这张表**不在本文件里**，在 scripts/lib/sim_procs.sh：
|
||
# 它同时被 run_sim_gz.sh（启动前的预检）使用，两处共用一份实现，不会漂移。
|
||
#
|
||
# 历史教训（都真实发生过，所以规则才写这么细）：
|
||
# - 漏过 robot_state_publisher：脚本说"已停止"，它还在跑；下一次启动时
|
||
# 继续发上一份 /robot_description 与 TF，表现成"改了描述却不生效"。
|
||
# - 漏过 static_transform_publisher：更阴 —— 它发的是 latched 的 /tf_static，
|
||
# 残留进程会一直把**旧相机外参**按在新仿真的 TF 树上。
|
||
# - 漏过 sim_gripper_adapter：残留的适配器与新适配器**同时**订阅 /joint_states、
|
||
# 同时发布 /gripper/status，服务调用还会被随机分给其中一个。
|
||
# 验收脚本读到的开口恒为 0 mm、指尖间距 0.03 mm（实际 127 mm），
|
||
# 看起来就像夹爪代码算错了 —— 这是最难查的一类残留。
|
||
#
|
||
# 所以停止分两步：
|
||
# ① 主通道：run_sim_gz.sh 用 setsid 起仿真，PGID 记在 log_sim/sim.pgid，
|
||
# `kill -TERM -PGID` 一次带走整组（**不依赖任何模式串**）；
|
||
# ② 兜底：模式扫描（点名表 + install_sim/ 兜底），收拾历史孤儿与
|
||
# ign 自己 fork 出去的进程；杀完再扫一遍，有残留就报出来并非零退出。
|
||
# shellcheck source=lib/sim_procs.sh
|
||
source "$(dirname "${BASH_SOURCE[0]}")/lib/sim_procs.sh"
|
||
|
||
PGID="$(sim_pgid_read)"
|
||
if [ -n "$PGID" ] && sim_pgid_alive "$PGID"; then
|
||
if ! sim_pgid_leader_is_ours "$PGID"; then
|
||
echo "⚠ log_sim/sim.pgid 里的进程组 $PGID 看起来不是仿真栈（PID 可能被复用），忽略它。"
|
||
PGID=""
|
||
fi
|
||
else
|
||
PGID=""
|
||
fi
|
||
|
||
if [ "$LIST_ONLY" = "1" ]; then
|
||
if [ -n "$PGID" ]; then
|
||
echo "仿真的进程组：$PGID（kill -TERM -$PGID 可整组停掉）"
|
||
ps -o pid=,args= -g "$PGID" 2>/dev/null | cut -c1-150
|
||
fi
|
||
fi
|
||
|
||
# 模式扫描的候选（--list 时先打印，再决定动不动手）。
|
||
mapfile -t targets < <(sim_proc_targets)
|
||
|
||
if [ "${#targets[@]}" = "0" ] && [ -z "$PGID" ]; then
|
||
echo "没有正在运行的仿真进程（已排除本脚本自身与调用它的 shell）。"
|
||
exit 0
|
||
fi
|
||
|
||
if [ -n "$PGID" ] && [ "$LIST_ONLY" != "1" ]; then
|
||
echo "停止仿真进程组 $PGID（整组，含不在 install_sim/ 下的节点）…"
|
||
sim_pgid_stop "$PGID"
|
||
# 组信号发完后重新扫一遍：可能还有孤儿。
|
||
mapfile -t targets < <(sim_proc_targets)
|
||
fi
|
||
|
||
if [ "${#targets[@]}" = "0" ]; then
|
||
echo "仿真已停止。"
|
||
exit 0
|
||
fi
|
||
|
||
echo "将停止以下进程："
|
||
for pid in "${targets[@]}"; do
|
||
printf ' %-7s %s\n' "$pid" "$(ps -o args= -p "$pid" 2>/dev/null | cut -c1-140)"
|
||
done
|
||
|
||
if [ "$LIST_ONLY" = "1" ]; then
|
||
echo "（--list：未动手）"
|
||
exit 0
|
||
fi
|
||
|
||
for pid in "${targets[@]}"; do
|
||
kill -TERM "$pid" 2>/dev/null
|
||
done
|
||
|
||
# 给 HAL 一点时间退出。Gazebo 的渲染线程与 DART 的求解线程不会立刻收尾，
|
||
# 立刻 KILL 有可能留下共享内存段，下一次启动表现成"渲染初始化失败"。
|
||
for _ in $(seq 1 10); do
|
||
alive=0
|
||
for pid in "${targets[@]}"; do
|
||
kill -0 "$pid" 2>/dev/null && alive=1
|
||
done
|
||
[ "$alive" = "0" ] && break
|
||
sleep 0.5
|
||
done
|
||
|
||
for pid in "${targets[@]}"; do
|
||
if kill -0 "$pid" 2>/dev/null; then
|
||
echo " PID $pid 没响应 TERM，用 KILL"
|
||
kill -KILL "$pid" 2>/dev/null
|
||
fi
|
||
done
|
||
|
||
# ---------------------------------------------------------------------------
|
||
# 复查：杀完再扫一遍。
|
||
# 这一步是为了让"表漏了某个进程"变成**看得见的告警**，而不是
|
||
# "脚本说已停止、实际有残留"。robot_state_publisher 与 sim_gripper_adapter
|
||
# 都这么漏过，而后者导致的假象是"夹爪算错了"。
|
||
# ---------------------------------------------------------------------------
|
||
sleep 0.5
|
||
mapfile -t leftover < <(sim_proc_targets)
|
||
|
||
if [ "${#leftover[@]}" != "0" ]; then
|
||
echo "⚠ 仍有仿真相关进程存活（说明它既不在 lib/sim_procs.sh 的点名表里，"
|
||
echo " 也不在 install_sim/ 下 —— 请把它加进点名表，或让它从 install_sim/ 启动）："
|
||
for pid in "${leftover[@]}"; do
|
||
printf ' %-7s %s\n' "$pid" "$(ps -o args= -p "$pid" 2>/dev/null | cut -c1-140)"
|
||
done
|
||
echo "仿真已停止，但有残留 —— 残留会与下一次启动抢话题/TF。"
|
||
exit 3
|
||
fi
|
||
|
||
echo "仿真已停止。"
|