smart-inspection/scripts/ros-common.sh

#!/bin/bash
# ROS AGV 公共库
# 提供生产脚本共享的配置、清理与验证函数

set -euo pipefail

# ============================================================================
# 配置（可通过环境变量覆盖）
# ============================================================================
readonly AGV_PROJECT_DIR="${AGV_PROJECT_DIR:-$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)}"
readonly AGV_APP_DIR="${AGV_APP_DIR:-$AGV_PROJECT_DIR/agv_app}"
readonly AGV_ROS2_DIR="${AGV_ROS2_DIR:-$HOME/agv_pro_ros2}"
readonly ROS_SETUP="${ROS_SETUP:-/opt/ros/humble/setup.bash}"
readonly ROS_WORKSPACE_SETUP="${ROS_WORKSPACE_SETUP:-$AGV_ROS2_DIR/install/setup.bash}"
readonly SCAN_FIXER_DIR="${SCAN_FIXER_DIR:-$AGV_PROJECT_DIR/scan_fixer}"
readonly LOG_DIR="${LOG_DIR:-/tmp}"
readonly FASTRTPS_SHM_DIR="${FASTRTPS_SHM_DIR:-/dev/shm}"
readonly AGV_CONTROLLER_DEVICE="${AGV_CONTROLLER_DEVICE:-/dev/agvpro_controller}"
readonly UV_BIN="${UV_BIN:-}"
export ROS_DOMAIN_ID="${ROS_DOMAIN_ID:-1}"

# 日志文件
readonly BRINGUP_LOG="$LOG_DIR/ros2_bringup.log"
readonly NAV2_LOG="$LOG_DIR/ros2_nav2.log"
readonly CLOCK_LOG="$LOG_DIR/clock_publisher.log"
readonly SCAN_FIXER_LOG="$LOG_DIR/scan_fixer.log"
readonly FLASK_LOG="$LOG_DIR/agv_flask.log"

# ============================================================================
# 进程管理
# ============================================================================

# ROS 相关进程列表（用于清理）
readonly ROS_PROCESSES=(
    "ros2 launch agv_pro_bringup"
    "ros2 launch agv_pro_navigation2"
    "agv_pro_node"
    "lslidar_driver_node"
    "component_container"
    "robot_state_publisher"
    "fix_scan_timestamp"
    "clock_publisher"
    "python.*app.py"
    "uv run .*python app.py"
)

# 软杀所有进程
kill_all_soft() {
    echo "   软杀进程中..."
    for proc in "${ROS_PROCESSES[@]}"; do
        pkill -f "$proc" 2>/dev/null || true
    done
    sleep 2
}

# 硬杀所有进程
kill_all_hard() {
    echo "   强制终止中..."
    for proc in "${ROS_PROCESSES[@]}"; do
        pkill -9 -f "$proc" 2>/dev/null || true
    done
    sleep 1
}

# 统计匹配进程数
count_matching_processes() {
    local pattern=$1
    local current_pid=$$
    local shell_pid=$BASHPID
    local parent_pid=$PPID
    local count=0
    local pid
    local args

    while read -r pid args; do
        if [ -z "${pid:-}" ]; then
            continue
        fi
        if [ "$pid" = "$current_pid" ] || [ "$pid" = "$shell_pid" ] || [ "$pid" = "$parent_pid" ]; then
            continue
        fi
        if [[ "${args:-}" =~ $pattern ]]; then
            count=$((count + 1))
        fi
    done < <(ps -eo pid=,args=)

    echo "$count"
}

# 统计残留进程数
count_residual_processes() {
    count_matching_processes "agv_pro_node|lslidar_driver_node|component_container|fix_scan_timestamp|clock_publisher|app.py|ros2-daemon"
}

# ============================================================================
# FastRTPS 清理
# ============================================================================

# 清理 FastRTPS 共享内存
cleanup_fastrtps() {
    local count
    count=$(count_fastrtps_files)

    if [ "$count" -gt 0 ]; then
        rm -rf "$FASTRTPS_SHM_DIR"/fastrtps_*
        echo "   已清理 $count 个 FastRTPS 文件"
    else
        echo "   无 FastRTPS 文件残留"
    fi

    # 清理锁文件
    rm -f /tmp/scan_fixer.lock /tmp/clock_publisher.lock
}

# 统计 FastRTPS 文件数
count_fastrtps_files() (
    shopt -s nullglob
    local files=("$FASTRTPS_SHM_DIR"/fastrtps_*)
    echo "${#files[@]}"
)

# 载入 ROS2 环境后执行命令
ros2_exec() {
    bash -c '
        source "$1" || exit 1
        if [ -f "$2" ]; then
            source "$2" || exit 1
        fi
        export ROS_DOMAIN_ID="$3"
        shift 3
        "$@"
    ' _ "$ROS_SETUP" "$ROS_WORKSPACE_SETUP" "$ROS_DOMAIN_ID" "$@"
}

resolve_uv_bin() {
    if [ -n "$UV_BIN" ]; then
        if [ -x "$UV_BIN" ]; then
            echo "$UV_BIN"
            return 0
        fi
        return 1
    fi

    local candidate
    candidate=$(command -v uv 2>/dev/null || true)
    if [ -n "$candidate" ]; then
        echo "$candidate"
        return 0
    fi

    for candidate in "$HOME/.local/bin/uv" "$HOME/.cargo/bin/uv"; do
        if [ -x "$candidate" ]; then
            echo "$candidate"
            return 0
        fi
    done

    return 1
}

# ============================================================================
# ROS2 环境操作
# ============================================================================

ros2_topic_list() {
    ros2_exec ros2 topic list 2>/dev/null || true
}

ros2_topic_count() {
    local topics
    topics=$(ros2_exec timeout "${1:-5}" ros2 topic list 2>/dev/null || true)
    if [ -z "$topics" ]; then
        echo 0
    else
        printf '%s\n' "$topics" | sed '/^$/d' | wc -l
    fi
}

topic_exists() {
    ros2_topic_list | grep -Fxq "$1"
}

# 启动 ROS2 daemon
start_ros2_daemon() {
    echo "   启动 ros2 daemon..."
    rm -rf "$FASTRTPS_SHM_DIR"/fastrtps_* 2>/dev/null || true

    nohup bash -c '
        source "$1" || exit 1
        export ROS_DOMAIN_ID="$2"
        ros2 daemon start
    ' _ "$ROS_SETUP" "$ROS_DOMAIN_ID" >/dev/null 2>&1 &
    sleep 4

    # 等待 daemon 就绪
    for _ in $(seq 1 5); do
        if ros2_exec timeout 3 ros2 topic list &>/dev/null; then
            echo "   [OK] ros2 daemon 已就绪"
            return 0
        fi
        sleep 2
    done
    echo "   [WARN] ros2 daemon 可能有问题"
    return 1
}

# 停止 ROS2 daemon
stop_ros2_daemon() {
    echo "   重置 ros2 daemon..."
    pkill -f "ros2-daemon" 2>/dev/null || true
    pkill -9 -f "ros2-daemon" 2>/dev/null || true
    sleep 2

    bash -c '
        source "$1" || exit 0
        export ROS_DOMAIN_ID="$2"
        ros2 daemon stop
    ' _ "$ROS_SETUP" "$ROS_DOMAIN_ID" 2>/dev/null || true
    echo "   [OK] ros2 daemon 已重置"
}

# ============================================================================
# 等待/验证函数
# ============================================================================

# 等待话题出现
# 用法: wait_for_topic <话题名> <最大等待秒数>
wait_for_topic() {
    local topic=$1
    local max_wait=${2:-30}
    local elapsed=0

    while [ "$elapsed" -lt "$max_wait" ]; do
        if topic_exists "$topic"; then
            echo "   [OK] $topic 已上线"
            return 0
        fi
        sleep 2
        elapsed=$((elapsed + 2))
    done
    echo "   [WARN] $topic 未在 $max_wait 秒内上线"
    return 1
}

# 等待节点出现（匹配数量）
# 用法: wait_for_nodes <节点模式> <期望数量> <最大等待秒数>
wait_for_nodes() {
    local pattern=$1
    local expected=$2
    local max_wait=${3:-30}
    local elapsed=0
    local count=0

    while [ "$elapsed" -lt "$max_wait" ]; do
        local nodes
        nodes=$(ros2_exec timeout 8 ros2 node list --no-daemon --spin-time 3 2>/dev/null || true)
        count=$(printf '%s\n' "$nodes" | grep -cE "$pattern" || true)
        if [ "$count" -ge "$expected" ]; then
            echo "   [OK] 已检测到 $count 个节点"
            return 0
        fi
        sleep 2
        elapsed=$((elapsed + 2))
    done
    echo "   [WARN] 仅检测到 $count 个节点（期望 $expected 个）"
    return 1
}

# ============================================================================
# 日志/输出辅助
# ============================================================================

# 打印分节标题
section() {
    echo ""
    echo "=========================================="
    echo "  $1"
    echo "=========================================="
}

# 打印步骤
step() {
    echo ""
    echo "[$1] $2"
}

# 打印带状态的信息
info() {
    local status=$1
    local msg=$2
    if [ "$status" = "ok" ]; then
        echo "   [OK] $msg"
    elif [ "$status" = "warn" ]; then
        echo "   [WARN] $msg"
    elif [ "$status" = "err" ]; then
        echo "   [ERROR] $msg"
    else
        echo "   $msg"
    fi
}

# 显示日志尾部
show_log_tail() {
    local log_file=$1
    local lines=${2:-5}
    if [ -f "$log_file" ]; then
        echo "   --- 日志尾部 ($log_file) ---"
        tail -"$lines" "$log_file" 2>/dev/null | sed 's/^/   /' || true
    fi
}

# ============================================================================
# 完整清理流程
# ============================================================================

# 执行完整清理（供 stop_all.sh 使用）
full_cleanup() {
    section "Robot AGV 全量停止"

    step "1/5" "软杀所有相关进程"
    kill_all_soft

    step "2/5" "强制终止残留进程"
    kill_all_hard

    step "3/5" "重置 ros2 daemon"
    stop_ros2_daemon

    step "4/5" "清理 FastRTPS 共享内存"
    cleanup_fastrtps

    step "5/5" "验证清理结果"
    local proc_count=$(count_residual_processes)
    local fastrtps_left=$(count_fastrtps_files)

    echo "   残留进程数: $proc_count"
    echo "   FastRTPS 文件数: $fastrtps_left"

    if [ "$proc_count" -eq 0 ] && [ "$fastrtps_left" -eq 0 ]; then
        section "[OK] 停止完成 - 系统已完全清理"
    else
        section "[WARN] 停止完成 - 部分残留可能需要手动清理"
        echo ""
        echo "  手动清理命令（如需要）:"
        echo "    pkill -9 -f 'agv_pro_node|lslidar|component_container'"
        echo "    pkill -9 -f 'fix_scan_timestamp|app.py'"
        echo "    pkill -9 -f 'ros2-daemon'"
        echo "    rm -rf \"$FASTRTPS_SHM_DIR\"/fastrtps_*"
    fi
    echo ""
    echo "  现在可以安全运行 ./scripts/prod-backend.sh"
    echo ""
}

# ============================================================================
# 初始化（确保目录存在）
# ============================================================================
mkdir -p "$LOG_DIR"