#!/bin/bash
# AGV 拍摄系统完整启动脚本 - ROS2 + Flask
# 使用方法: ./start_all.sh

echo "=== 停止旧进程 ==="
pkill -f "ros2 launch agv_pro_bringup" 2>/dev/null
pkill -f "python.*app.py" 2>/dev/null
sleep 2

echo "=== 启动 ROS2 Bringup ==="
# Source ROS2 环境
source /opt/ros/humble/setup.bash
cd /home/elephant/agv_pro_ros2
source install/setup.bash

# 启动 ROS2 bringup (后台运行)
nohup ros2 launch agv_pro_bringup agv_pro_bringup.launch.py port_name:=/dev/agvpro_controller > /tmp/ros2_bringup.log 2>&1 &
ROS2_PID=$!
echo "ROS2 bringup started, PID: $ROS2_PID"

# 等待 ROS2 初始化 (AGV节点需要连接串口)
echo "等待 ROS2 初始化..."
sleep 5

# 检查 ROS2 节点是否启动
if source /opt/ros/humble/setup.bash && source ~/agv_pro_ros2/install/setup.bash && ros2 node list 2>/dev/null | grep -q agv_pro_node; then
    echo "✅ ROS2 AGV 节点已启动"
else
    echo "⚠️ ROS2 节点启动可能失败，请检查日志: /tmp/ros2_bringup.log"
fi

echo "=== 启动 Nav2 导航 ==="
source /opt/ros/humble/setup.bash
cd /home/elephant/agv_pro_ros2
source install/setup.bash

# 启动 Nav2 (后台运行)
nohup ros2 launch agv_pro_navigation2 navigation2_active.launch.py > /tmp/ros2_nav2.log 2>&1 &
NAV2_PID=$!
echo "Nav2 started, PID: $NAV2_PID"

sleep 10
echo "=== 激活 Nav2 Lifecycle ==="
ros2 lifecycle set /lifecycle_manager_navigation configure 2>&1 | tee -a /tmp/start_all.log
sleep 3
ros2 lifecycle set /lifecycle_manager_navigation activate 2>&1 | tee -a /tmp/start_all.log
sleep 3
echo "✅ Nav2 导航已启动并激活"

echo "=== 启动 Flask ==="
cd /home/elephant/work/agv_app
nohup python3 app.py > /tmp/agv_flask.log 2>&1 &
FLASK_PID=$!
echo "Flask started, PID: $FLASK_PID"

sleep 2
echo ""
echo "=== 启动完成 ==="
echo "ROS2 bringup log: /tmp/ros2_bringup.log"
echo "Nav2 log: /tmp/ros2_nav2.log"
echo "Flask log: /tmp/agv_flask.log"
echo ""
echo "检查状态:"
echo "  ros2 node list"
echo "  curl http://localhost:5000/api/status"