#!/bin/bash
# ============================================================
# stop_all.sh - 关闭 AGV 拍摄系统所有相关进程
# 版本: v2.0
# 修复:
#   - v2.0: 添加 FastRTPS 清理 + ros2 daemon 重置
# ============================================================
set -e

echo "=========================================="
echo "  Robot AGV 全量停止"
echo "=========================================="
echo ""

# ---------- 1. 软杀所有相关进程 ----------
echo "[1/5] 软杀所有相关进程..."
pkill -f "python3 app.py" 2>/dev/null || true
pkill -f "agv_pro_bringup" 2>/dev/null || true
pkill -f "agv_pro_navigation2" 2>/dev/null || true
pkill -f "agv_pro_node" 2>/dev/null || true
pkill -f "lslidar_driver_node" 2>/dev/null || true
pkill -f "component_container" 2>/dev/null || true
pkill -f "fix_scan_timestamp" 2>/dev/null || true
pkill -f "clock_publisher" 2>/dev/null || true
pkill -f "robot_state_publisher" 2>/dev/null || true
pkill -f "start_all.sh" 2>/dev/null || true
sleep 2

# ---------- 2. 硬杀确保干净 ----------
echo "[2/5] 硬杀残留进程..."
pkill -9 -f "app.py" 2>/dev/null || true
pkill -9 -f "agv_pro_node" 2>/dev/null || true
pkill -9 -f "lslidar_driver_node" 2>/dev/null || true
pkill -9 -f "component_container" 2>/dev/null || true
pkill -9 -f "fix_scan_timestamp" 2>/dev/null || true
pkill -9 -f "agv_pro_bringup" 2>/dev/null || true
pkill -9 -f "agv_pro_navigation2" 2>/dev/null || true
sleep 1

# ---------- 3. 【关键】清理 FastRTPS 共享内存 ----------
echo "[3/5] 清理 FastRTPS 共享内存..."
FASTRTPS_COUNT=$(ls /dev/shm/fastrtps_* 2>/dev/null | wc -l || echo 0)
if [ "$FASTRTPS_COUNT" -gt 0 ]; then
    rm -rf /dev/shm/fastrtps_*
    echo "   已清理 $FASTRTPS_COUNT 个 FastRTPS 文件"
else
    echo "   无 FastRTPS 文件残留"
fi

# 清理 scan_fixer 锁文件
rm -f /tmp/scan_fixer.lock
rm -f /tmp/clock_publisher.lock
echo "   ✅ FastRTPS 清理完成"

# ---------- 4. 【关键】重置 ros2 daemon ----------
echo "[4/5] 重置 ros2 daemon..."
pkill -f "ros2-daemon" 2>/dev/null || true
pkill -9 -f "ros2-daemon" 2>/dev/null || true
sleep 2
source /opt/ros/humble/setup.bash 2>/dev/null || true
ros2 daemon stop 2>/dev/null || true
echo "   ✅ ros2 daemon 已重置"

# ---------- 5. 验证清理结果 ----------
echo "[5/5] 验证清理结果..."
PROC_COUNT=$(ps aux | grep -E 'agv_pro_node|lslidar_driver_node|component_container|fix_scan_timestamp|clock_publisher|app.py|ros2-daemon' | grep -v grep | wc -l || echo 0)
FASTRTPS_LEFT=$(ls /dev/shm/fastrtps_* 2>/dev/null | wc -l || echo 0)

echo "   残留进程数: $PROC_COUNT"
echo "   FastRTPS 文件数: $FASTRTPS_LEFT"

if [ "$PROC_COUNT" -eq 0 ] && [ "$FASTRTPS_LEFT" -eq 0 ]; then
    echo ""
    echo "=========================================="
    echo "  ✅ 停止完成 - 系统已完全清理"
    echo "=========================================="
else
    echo ""
    echo "=========================================="
    echo "  ⚠️ 停止完成 - 部分残留可能需要手动清理"
    echo "=========================================="
    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 /dev/shm/fastrtps_*"
fi
echo ""
echo "  现在可以安全运行 ./start_all.sh"
echo ""