#!/usr/bin/env python3
"""
Demo 04: 多机依次起飞与依次降落
演示多架无人机按顺序依次起飞和降落的控制
"""
from ros2_px4_swarm_manager import SimpleSwarmDemo


class MultiTakeoffLandSequence(SimpleSwarmDemo):
    """多机依次起飞降落演示"""
    
    def __init__(self):
        super().__init__(uav_ids=[1, 2, 3], refresh_rate=0.5)
        self.height = 3.0
        
    def run(self):
        """执行任务"""
        self.log("=" * 50)
        self.log("Demo 04: 多机依次起飞与降落")
        self.log(f"无人机: {self.uav_ids}")
        self.log("=" * 50)
        
        # 1. 依次起飞
        self.log("\n🚀 步骤 1: 依次起飞")
        for uav_id in self.uav_ids:
            self.log(f"  🚁 UAV {uav_id} 正在起飞...")
            # 使用阻塞模式，等这架起飞完成再起飞下一架
            if not self.takeoff(uav_id, self.height, blocking=True):
                self.log(f"  ❌ UAV {uav_id} 起飞失败")
                continue
            self.log(f"  ✅ UAV {uav_id} 起飞完成")
            self.sleep(1)  # 间隔 1 秒
        
        # 2. 悬停
        self.log("\n⏸️ 步骤 2: 全体悬停 5 秒")
        self.sleep(5)
        
        # 3. 依次降落
        self.log("\n🛬 步骤 3: 依次降落")
        for uav_id in self.uav_ids:
            self.log(f"  🚁 UAV {uav_id} 正在降落...")
            self.land(uav_id)
            self.sleep(3)  # 每架间隔 3 秒降落
            self.log(f"  ✅ UAV {uav_id} 降落完成")
        
        self.log("\n✅ 任务完成！")


if __name__ == '__main__':
    MultiTakeoffLandSequence().start()
