ros_contorl/scripts/lib/sim_procs.sh

#!/usr/bin/env bash
# =============================================================================
# scripts/lib/sim_procs.sh —— 仿真进程扫描（被 stop_sim_gz.sh / run_sim_gz.sh 共用）
#
# 为什么单独抽一层
# ----------------
# "哪些进程算仿真栈"这件事以前只写在 stop_sim_gz.sh 里。新增一个仿真节点
# （sim_gripper_adapter）时**忘了同步那张模式表**，于是：
#
#     脚本说"仿真已停止" → 其实残留了一个夹爪适配器
#     → 下一次启动后**两个适配器同时订阅 /joint_states、同时发布 /gripper/status**
#     → 服务调用被负载均衡到任意一个；TF 也有两份来源
#     → 验收脚本读到的开口/指尖间距全是乱的（实测：开口恒为 0 mm，
#        指尖间距 0.03 mm，而实际是 127 mm）
#
# 这个坑的可怕之处在于**它看起来像夹爪代码写错了**。所以这里做三件事：
#
#   1. 两份脚本共用同一个扫描实现 —— 不再各写一份会漂移的模式表；
#   2. 停止的主通道改成**进程组**：仿真整栈用 setsid 起在自己的会话里，
#      PGID 记在 log_sim/sim.pgid，停止时 kill 整组，不依赖模式匹配；
#   3. 模式扫描当兜底（对付 ign 自己 fork 的进程与历史孤儿）。扫描本身是
#      "点名表 + install_sim/ 兜底"：凡命令行含 ``<仓库>/install_sim/``
#      的进程都算仿真进程，所以**以后再加新节点，不改这个文件也能被扫到**。
#      点名表只用于少数"不在 install_sim 里"的进程（Gazebo 本体、桥接、
#      robot_state_publisher、static_transform_publisher）。
#
# 用法（在 `set -uo pipefail` 之后 source）：
#     source "$(dirname "$0")/lib/sim_procs.sh"
#     mapfile -t targets < <(sim_proc_targets)     # 只输出 PID（已排除自身/祖先）
#     sim_proc_scan                                 # 输出 "PID<TAB>命令行"
#     sim_pgid_read / sim_pgid_leader_is_ours / sim_pgid_stop
# =============================================================================

# 兜底模式：仿真安装空间。用绝对路径前缀，避免误伤 install/（真机那边）。
# ⚠️ 路径要按 awk 正则转义：路径里可能有 . + - 之类字符，不转义的话
#    "." 会匹配任意字符（匹配面变大 = 误杀无关进程）。
_sim_install_dir() {
    printf '%s/install_sim/' "$(_sim_repo_root)"
}

_sim_regex_escape() {
    printf '%s' "$1" | sed -e 's/[][\.^$*+?(){}|\\]/\\&/g'
}

# 点名表：列**不在 install_sim/ 下**、但属于仿真栈的进程。
#   ign gazebo / gz sim   Gazebo 服务端与 GUI（conda Fortress 里可执行名是 ign gazebo）
#   parameter_bridge      相机/触觉/clock 的 ros_gz_bridge（在 conda env 的 lib 下）
#   robot_state_publisher 发 TF 与 /robot_description（同上，不在 install_sim）
#   static_transform_publisher  相机的静态外参（同上是 conda env 里的可执行文件）
#   controller_manager.*spawner / gz_ros2_control  控制器与它的 spawner
#
# ⚠️ 这几个必须**点名**：它们的可执行文件来自 conda 环境或 ROS 发行版，
#    命令行里没有 install_sim/ 字样，兜底规则扫不到。
#    实测教训：只留兜底规则时 robot_state_publisher 会漏，而它的症状是
#    "脚本说已停止，它还在发上一份 TF 与 /robot_description"。
#    更阴的是 static_transform_publisher：它发的是**latched**的 /tf_static，
#    残留进程会一直发**旧的外参**，表现成"改了相机安装位置却不生效"。
#
# 有意**不**包含 rviz2：headless 模式不起 RViz，用户可能开着 RViz 看别的东西，
# 写进来会误杀无关进程 —— 误杀比漏杀更糟。
SIM_PROC_NAMED='ign gazebo|gz sim|parameter_bridge|ros_gz_bridge|robot_state_publisher|static_transform_publisher|controller_manager.*spawner|gz_ros2_control'

# ---------------------------------------------------------------------------
# 进程组：精确停止的主通道
# ---------------------------------------------------------------------------
# 模式匹配永远是"猜"，上面那张表就漏过好几次。更可靠的做法是让**整个仿真栈
# 待在自己的进程组里**：启动时用 setsid 新开一个会话，PID 即 PGID，写进
# log_sim/sim.pgid；停止时 `kill -TERM -<PGID>` 一次性带走整组，
# 不依赖任何模式串。模式扫描退化成兜底（对付 ign 自己 fork 出去的进程、
# 以及历史遗留的孤儿）。
#
# ⚠️ 文件里的 PGID 可能是**过时的**（那次仿真的进程早退出了，PID 被别人复用）。
#    所以用之前必须确认那个进程确实是我们的仿真栈 —— 见 sim_pgid_leader_is_ours。
sim_pgid_file() {
    printf '%s/log_sim/sim.pgid' "$(_sim_repo_root)"
}

_sim_repo_root() {
    cd "$(dirname "${BASH_SOURCE[0]}")/../.." && pwd
}

# 读 pgid 文件里的 PGID；没有/格式不对 → 输出空。
sim_pgid_read() {
    local f pgid
    f="$(sim_pgid_file)"
    [ -f "$f" ] || return 0
    read -r pgid <"$f" 2>/dev/null || return 0
    case "${pgid:-}" in
        ''|*[!0-9]*) return 0 ;;   # 不是纯数字 → 当作无效
    esac
    printf '%s' "$pgid"
}

# 该 PGID 的组长进程是否确实是仿真栈（防止 PID 复用导致误杀无关进程组）。
sim_pgid_leader_is_ours() {
    local pgid="$1" args
    args="$(ps -o args= -p "$pgid" 2>/dev/null || true)"
    [ -n "$args" ] || return 1
    printf '%s' "$args" | grep -Eq 'arm_sim_gazebo|gazebo_sim|ign gazebo|gz sim|ros2 launch|ros2 run'
}

# 组是否还活着（任一组员存活即可）。
sim_pgid_alive() {
    local pgid="$1"
    kill -0 -- "-$pgid" 2>/dev/null
}

# 停止整组：先 TERM 等一下，仍在就 KILL。成功输出 "stopped"。
sim_pgid_stop() {
    local pgid="$1" i
    if ! sim_pgid_alive "$pgid"; then
        rm -f "$(sim_pgid_file)"
        return 0
    fi
    kill -TERM -- "-$pgid" 2>/dev/null
    for i in $(seq 1 20); do
        sim_pgid_alive "$pgid" || break
        sleep 0.5
    done
    if sim_pgid_alive "$pgid"; then
        echo "进程组 $pgid 没响应 TERM，用 KILL" >&2
        kill -KILL -- "-$pgid" 2>/dev/null
        sleep 0.5
    fi
    rm -f "$(sim_pgid_file)"
}

sim_proc_patterns() {
    printf '%s|%s' "$SIM_PROC_NAMED" "$(_sim_regex_escape "$(_sim_install_dir)")"
}

# 扫描：输出 "PID<TAB>命令行"。排除本进程/祖先/扫描用的 awk 自身。
#
# ⚠️ 两个必须踩过的坑（原 stop_sim_gz.sh 的注释，这里保留）：
#   1. `pkill -f <模式>` 会匹配到自己所在的 shell（命令行里含那个模式），
#      表现成"命令立刻返回、什么都没杀"。
#   2. 模式串用**环境变量**传给 awk，不用 `-v pat=...`：用了 -v 的话 awk 自己的
#      命令行里就含整串模式，于是它匹配到自己，复查时多报一个幽灵 PID。
sim_proc_scan() {
    export PAT="$(sim_proc_patterns)"
    export ME="${SIM_PROC_SELF:-$$}"
    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）
            if (args ~ /sim_procs\.sh/) next  # 本库自己（含 grep/sed 的读数）
            if (args ~ /stop_sim_gz\.sh/) next
            if (args ~ /run_sim_gz\.sh/) next
            if (args ~ /^\s*awk/) next
            print pid "\t" args
        }'
}

# 排除自己与全部祖先，只输出 PID。
sim_proc_targets() {
    local exclude=() p=$$ pid _rest e skip
    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
    while read -r pid _rest; do
        [ -z "${pid:-}" ] && continue
        skip=0
        for e in "${exclude[@]}"; do
            [ "$pid" = "$e" ] && skip=1 && break
        done
        [ "$skip" = "1" ] && continue
        echo "$pid"
    done < <(sim_proc_scan)
}