#!/usr/bin/env python3
"""
Demo 03: 多机同时起飞与同时降落
演示多架无人机同时起飞和同时降落的控制
"""
from ros2_px4_swarm_manager import SimpleSwarmDemo


class MultiTakeoffLandTogether(SimpleSwarmDemo):
    """多机同时起飞降落演示"""
    
    def __init__(self):
        # 控制 3 架无人机
        super().__init__(uav_ids=[1, 2, 3], refresh_rate=0.5)
        self.height = 3.0
        
    def run(self):
        """执行任务"""
        self.log("=" * 50)
        self.log("Demo 03: 多机同时起飞与降落")
        self.log(f"无人机: {self.uav_ids}, 高度: {self.height}m")
        self.log("=" * 50)
        
        # 1. 同时起飞（非阻塞模式，同时执行）
        self.log("\n🚀 步骤 1: 三机同时起飞")
        for uav_id in self.uav_ids:
            # 使用非阻塞模式，立即返回，实现"同时"效果
            self.controller.takeoff(uav_id, self.height, blocking=False)
            self.log(f"  🚁 UAV {uav_id} 起飞命令已发送")
        
        # 等待所有无人机起飞完成
        self.log("⏳ 等待 8 秒让无人机起飞...")
        self.sleep(8)
        
        # 2. 悬停
        self.log("\n⏸️ 步骤 2: 悬停 5 秒")
        self.sleep(5)
        
        # 3. 同时降落
        self.log("\n🛬 步骤 3: 三机同时降落")
        for uav_id in self.uav_ids:
            self.land(uav_id)
        
        # 等待降落完成
        self.sleep(8)
        
        self.log("\n✅ 任务完成！")


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