Refactor ROS startup scripts

2026-06-22 13:06:31 +08:00
parent 62cccfbcc6
commit 55f646053d
5 changed files with 480 additions and 345 deletions
@@ -0,0 +1,328 @@
+#!/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}"
+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" "$@"
+}
+
+# ============================================================================
+# 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
+
+    source "$ROS_SETUP" 2>/dev/null || true
+    ros2 daemon stop 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 ros2 node list 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"