#!/bin/bash

# 多无人机offboard控制启动脚本
# 用法: ./launch_multi_uav.sh [无人机数量]

# 设置默认无人机数量
NUM_UAVS=${1:-2}

echo "启动多无人机offboard控制程序..."
echo "无人机数量: $NUM_UAVS"

# 检查ROS 2环境
if [ -z "$ROS_DISTRO" ]; then
    echo "错误: 请先source ROS 2环境"
    echo "例如: source /opt/ros/humble/setup.bash"
    exit 1
fi

# 检查工作空间
if [ ! -f "install/setup.bash" ]; then
    echo "错误: 请先构建工作空间"
    echo "运行: colcon build"
    exit 1
fi

# Source工作空间
source install/setup.bash

# 启动多无人机控制节点
echo "启动多无人机控制节点..."
python3 src/px4_ros_com/src/examples/offboard_py/multi_uav_offboard_control.py $NUM_UAVS