P5c-6 场景物料落地 + Gazebo 中文手册：工件/通止规模型、场景校验、两处静默失效修复
一、场景物料（新增）
devices/sim/gazebo/models/workpiece/ 带三个孔的铝板。碰撞体是"带方孔的平板"，
视觉额外加两个 φ14/φ7 亮环 —— 因为真机的 HoughCircles 就是靠这两个亮环找孔，
只按碰撞体建视觉会让"孔检测"在仿真里永远失败，而那看起来像算法问题。
devices/sim/gazebo/models/gauge/ 通止规：通端/套环/握柄/止端四段，各段异色
便于肉眼判断方向。接触刚度刻意比台面软 —— 位置控制器会"刚性压进去"，
用默认刚度会被弹飞（注释里写了原因，不是随手抄的参数）。
devices/sim/gazebo/models/gauge_stand/ 规座（static）。规直接站在台面上会立刻倒，
加个方孔底座把它立住。
三个模型都是 model.sdf + model.config，原点在**底面中心**，单位米。
二、场景配置与校验（新增）
config/scene.yaml 物体不再写死在 world 里，改为 scene.yaml + 启动时动态 spawn。
理由：world 是"环境"（地面/台面/相机/系统插件），场景是"这一批活做什么"。
动 world 的风险面太大 —— 它只要丢一个系统声明，所有模型都 spawn 不进去（见旧坑 2）。
含 operating（相机到工件的距离等作业条件）与 gates_px（像素门槛）。
sim_scene.py 摆放校验：从 SDF **逐碰撞体**解析 AABB（不抄尺寸，避免漂移），
检查原点在底面 / 不穿台面 / 不出界 / 物体之间不嵌入。
重叠判定必须逐碰撞体三维判，不能拿总包围盒相交 ——
正常装配里本来就有嵌套（规立在规座方孔里），粗判会误报，
而误报的后果是人把检查整个关掉，之后真重叠也抓不到。
另有成像几何：把亮环/孔间距按相机距离换算成像素，与门槛比对。
scripts/check_sim_scene.py 验收工具（退出码可用）：摆放 + 门槛漂移 + 成像几何 +
悬停高度 vs 工作空间 + 手外相机是否**完整**看得见工件 + Gazebo 实际位姿。
三、修掉两处静默失效（都是"看起来一切正常"）
1) parse_spawned_poses 一直是**死代码**：它按 "x: N / y: N / z: N" 解析，
而 Fortress 实际输出是方括号两行（XYZ / RPY）。对不上 → 永远返回空 →
调用方按设计"降级成跳过位姿核对" → 位姿检查**从来没跑过**，
而日志只说"跳过"，看起来像正常降级。
现在两种格式都支持，并用真实抓下来的输出样本做回归测试。
教训：宽容降级会把"没检查"伪装成"检查通过"。
2) stop_sim_gz.sh 的模式表里漏了 robot_state_publisher（而注释里写着它有覆盖）：
脚本报"已停止"，它却还在发上一份 /robot_description 与 TF，
下一次启动就会"改了描述却不生效"。
顺带修掉同源的两个问题：awk 用 -v 传模式导致它匹配到自己、每次复查都多报
一个空命令行的幽灵 PID；以及"杀的时候"和"复查的时候"两份扫描实现会各自漂移。
现在共用同一个 scan_pids()，模式改用环境变量传递，并在杀完之后**再扫一遍**：
有残留就报出来并非零退出 —— 下次再有漏项，不再静默。
实测：--list 列出全套 9 个进程；停止后 rc=0 且 pgrep 干净。
四、相机：D405 分辨率对齐真机
仿真的 D405 此前渲染 640x480，而真机跑 848x480（fx 337 而非 447）。后果是
58mm 的两孔间距成像只剩 85px，低于 hole_distance_min=90 ——
孔检测在仿真里根本不可能通过，而现象看起来像"算法不行"。
已把 xacro 与 cameras.yaml 一起改成 848x480，并把启动时的比对从"只比视场"
扩成"视场 + 图幅"（sim_cameras.check_camera_consistency），
不一致时说明它对像素门槛的影响。
五、文档
devices/sim/gazebo/README.md 重写为中文手册：使用方法 / 建模要求 / 导入格式 /
配置方式（含"改哪里才真的生效"的接线表）/ 11 条实测坑（每条都有"怎么发现"）。
manifest.yaml 的进度、assets、description、坑清单与本轮状态对齐。
scripts/README.md 补两个验收工具与停止脚本；根 README 补仿真入口与手册链接。
cameras.yaml 里一处过期的函数名（check_fov_consistency）修正。
六、测试
新增 test_sim_scene.py（79 条，此前 sim_scene.py 完全没有单测 —— 这正是
位姿解析死掉却无人发现的原因）与 test_check_sim_scene.py（21 条），
含三条回归：world_boxes 的偏移碰撞体不能被对称化放大（假重叠）、
center_bore 不能被 _bore 后缀吃掉、Fortress 位姿输出格式。
全量：src/arm_sim_gazebo + arm_interfaces + arm_application 共 515 通过。
同时附上 config/site.example.yaml 的重新生成（此前 site.yaml 的注释改动没同步，
它的 parity 测试是红的）。
Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
arm_dev
Executable
+155
@@ -0,0 +1,155 @@
|
||||
#!/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
|
||||
|
||||
# 这些模式覆盖仿真栈的全部进程：
|
||||
# gz sim / ign gazebo 服务端与 GUI（conda 的 Fortress 里可执行名是 ign gazebo）
|
||||
# gazebo_sim.launch.py ros2 launch 的壳
|
||||
# gazebo_sim_launcher 入口可执行文件（展开描述后转交 launch）
|
||||
# parameter_bridge 相机/触觉/clock 的桥接
|
||||
# sim_camera_adapter / sim_tactile_adapter 两个适配器
|
||||
# robot_state_publisher 发 TF 与 /robot_description
|
||||
# controller_manager.*spawner / gz_ros2_control 控制器
|
||||
#
|
||||
# ⚠️ 这里漏过一个（robot_state_publisher），症状是"脚本说已停止，但它还在跑"：
|
||||
# 下一次启动时它继续发上一份 /robot_description 与 TF，而两个进程的
|
||||
# 参数可能不同 —— 表现成"改了描述却不生效"，极难往"有个残留进程"上想。
|
||||
# 所以脚本末尾加了一次**复查**：杀完再扫一遍，有残留就报出来并非零退出。
|
||||
# 加进新进程时，务必确认复查也能扫到它。
|
||||
#
|
||||
# 有意**不**包含 rviz2：本仿真栈的 headless 模式不起 RViz，而用户可能同时
|
||||
# 开着 RViz 看别的东西。把它写进来会误杀无关进程，比漏杀更糟。
|
||||
PATTERNS='ign gazebo|gz sim|gazebo_sim\.launch\.py|gazebo_sim_launcher|parameter_bridge|sim_camera_adapter|sim_tactile_adapter|robot_state_publisher|controller_manager.*spawner|gz_ros2_control'
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# 扫描：输出 "PID<TAB>命令行"，供两次调用（收集 / 复查）共用。
|
||||
#
|
||||
# 两处必须共用同一个实现，否则"复查用的模式"和"杀的时候用的模式"会各自漂移，
|
||||
# 而漂移的表现就是"说已停止、其实有残留"——正是这个脚本要防的事。
|
||||
#
|
||||
# ⚠️ 模式串用**环境变量**传给 awk，不用 `-v pat=...`：
|
||||
# 用了 -v 的话，awk 自己的命令行里就含整串模式，于是它匹配到自己。
|
||||
# 这是 pkill -f 那个坑的同款，只是换了个位置 —— 实测表现为每次复查都会
|
||||
# 多报出一个"没有命令行"的幽灵 PID（awk 已经退出，ps 查不到它了）。
|
||||
#
|
||||
# 另外用 ps 而不是 pgrep -f：ps 的输出里可以用 awk 精确控制字段与排除规则。
|
||||
scan_pids() {
|
||||
# ⚠️ 必须 export：`VAR=x ps ... | awk` 只会把 VAR 给 ps，awk 看不到。
|
||||
export PAT="$PATTERNS"
|
||||
ps -eo pid=,ppid=,args= | awk '
|
||||
BEGIN { pat = ENVIRON["PAT"]; me = ENVIRON["ME"] }
|
||||
$0 ~ pat {
|
||||
if (NF < 3) next # 没有 args 字段（僵尸/内核线程）：跳过
|
||||
pid = $1; ppid = $2
|
||||
args = substr($0, index($0, $3))
|
||||
if (ppid == me) next # 本脚本的直接子进程（进程替换 fork 出的子 shell）
|
||||
if (args ~ /stop_sim_gz\.sh/) next # 本脚本自己 / 调用它的壳的文本
|
||||
if (args ~ /^\s*awk/) next # 扫描用的 awk 本身
|
||||
print pid "\t" args
|
||||
}'
|
||||
}
|
||||
export ME=$$
|
||||
|
||||
# 排除自己与全部祖先 —— 这是本脚本存在的核心理由（见文件头）。
|
||||
exclude=()
|
||||
p=$$
|
||||
while [ "$p" -gt 1 ] 2>/dev/null; do
|
||||
exclude+=("$p")
|
||||
p=$(ps -o ppid= -p "$p" 2>/dev/null | tr -d ' ')
|
||||
[ -z "$p" ] && break
|
||||
done
|
||||
|
||||
# 从扫描结果里剔掉自己/祖先，只输出 PID。
|
||||
# 用 `cut -f1` 而不是 read 到数组再拼接，避免命令行里再出现长的进程描述。
|
||||
filter_targets() {
|
||||
local pid
|
||||
while read -r pid _rest; do
|
||||
[ -z "$pid" ] && continue
|
||||
local skip=0
|
||||
for e in "${exclude[@]}"; do
|
||||
[ "$pid" = "$e" ] && skip=1 && break
|
||||
done
|
||||
[ "$skip" = "1" ] && continue
|
||||
echo "$pid"
|
||||
done
|
||||
}
|
||||
|
||||
mapfile -t SCAN < <(scan_pids)
|
||||
mapfile -t targets < <(printf '%s\n' "${SCAN[@]:-}" | filter_targets)
|
||||
|
||||
if [ "${#targets[@]}" = "0" ]; then
|
||||
echo "没有正在运行的仿真进程（已排除本脚本自身与调用它的 shell）。"
|
||||
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 就这么漏过一次。
|
||||
# ---------------------------------------------------------------------------
|
||||
sleep 0.5
|
||||
mapfile -t SCAN2 < <(scan_pids)
|
||||
mapfile -t leftover < <(printf '%s\n' "${SCAN2[@]:-}" | filter_targets)
|
||||
|
||||
if [ "${#leftover[@]}" != "0" ]; then
|
||||
echo "⚠ 仍有仿真相关进程存活（说明上面的模式没覆盖它，请把它加进 PATTERNS）："
|
||||
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 "仿真已停止。"
|
||||
Reference in New Issue
Block a user