#!/bin/bash
# 启动巡检脚本

echo ==========================================
echo GO2 机器狗巡检启动脚本
echo ==========================================

# Source 环境
source /opt/ros/humble/setup.bash
source ~/unitree_ros2/cyclonedds_ws/install/setup.bash
source ~/go2_robot_system/install/setup.bash

# 检查航点文件
ROUTE_FILE=~/go2_robot_system/config/ros2_patrol_routes/default.yaml

if [ ! -f "$ROUTE_FILE" ]; then
    echo "❌ 错误：航点文件不存在: $ROUTE_FILE"
    exit 1
fi

# 显示航点信息
echo ""
echo "📍 航点文件: $ROUTE_FILE"
echo ""
echo "航点列表:"
grep -A 5 "name:" "$ROUTE_FILE" | grep -E "(name|x:|y:|yaw:)" | head -20

# 询问是否开始
echo ""
read -p "是否开始巡检？(y/n): " -n 1 -r
echo
if [[ ! $REPLY =~ ^[Yy]$ ]]; then
    echo "已取消"
    exit 0
fi

echo ""
echo "🚀 启动巡检节点..."
echo ""

# 启动巡检
ros2 run go2_patrol patrol_executor_node --ros-args -p autostart:=true