AirTrapMine/src/GUI/mission_planner.py

#!/usr/bin/env python3
"""
飛行任務規劃模組
新增：Circle Formation 和 Leader-Follower
"""
import math
from typing import List, Tuple, Optional, Dict, Any
from enum import Enum


class MissionType(Enum):
    """任務類型"""
    M_FORMATION = "m_formation"
    CIRCLE_FORMATION = "circle_formation"
    LEADER_FOLLOWER = "leader_follower"


class CoordinateConverter:
    """GPS 座標與 Local 座標的轉換器"""
    
    def __init__(self, origin_lat: float, origin_lon: float, origin_alt: float = 0):
        self.origin_lat = origin_lat
        self.origin_lon = origin_lon
        self.origin_alt = origin_alt
        self.R = 6371000.0
    
    def gps_to_local(self, lat: float, lon: float, alt: float) -> Tuple[float, float, float]:
        lat_rad = math.radians(lat)
        lon_rad = math.radians(lon)
        origin_lat_rad = math.radians(self.origin_lat)
        origin_lon_rad = math.radians(self.origin_lon)
        
        dlat = lat_rad - origin_lat_rad
        dlon = lon_rad - origin_lon_rad
        
        x = self.R * dlon * math.cos((lat_rad + origin_lat_rad) / 2)
        y = self.R * dlat
        z = alt - self.origin_alt
        
        return x, y, z
    
    def local_to_gps(self, x: float, y: float, z: float) -> Tuple[float, float, float]:
        origin_lat_rad = math.radians(self.origin_lat)
        origin_lon_rad = math.radians(self.origin_lon)
        
        lat_rad = origin_lat_rad + (y / self.R)
        lon_rad = origin_lon_rad + (x / (self.R * math.cos((lat_rad + origin_lat_rad) / 2)))
        
        lat = math.degrees(lat_rad)
        lon = math.degrees(lon_rad)
        alt = self.origin_alt + z
        
        return lat, lon, alt


class FormationPlanner:
    """隊形規劃器"""
    
    def __init__(self, spacing: float = 5.0, 
                 base_altitude: float = 10.0, 
                 altitude_diff: float = 2.0):
        self.spacing = spacing
        self.base_altitude = base_altitude
        self.altitude_diff = altitude_diff
        self.current_origin = None
        self.converter = None
    
    def plan_formation_mission(self, 
                              drone_gps_positions: List[Tuple[float, float, float]], 
                              target_gps: Tuple[float, float, float],
                              mission_type: MissionType = MissionType.M_FORMATION,
                              params: Optional[Dict[str, Any]] = None) -> Tuple[
                                  List[Tuple[float, float, float]], 
                                  List[Tuple[float, float, float]],
                                  Tuple[float, float, float]]:
        """
        規劃兩階段隊形任務
        
        Args:
            drone_gps_positions: 當前無人機 GPS 位置列表
            target_gps: 目標點 GPS 座標
            mission_type: 任務類型
            params: 任務參數
            
        Returns:
            (stage1_gps, stage2_gps, origin)
        """
        if len(drone_gps_positions) == 0:
            raise ValueError("無人機位置列表不能為空")
        
        # 計算中央點
        center_lat = sum(pos[0] for pos in drone_gps_positions) / len(drone_gps_positions)
        center_lon = sum(pos[1] for pos in drone_gps_positions) / len(drone_gps_positions)
        center_alt = sum(pos[2] for pos in drone_gps_positions) / len(drone_gps_positions)
        
        self.current_origin = (center_lat, center_lon, center_alt)
        self.converter = CoordinateConverter(center_lat, center_lon, center_alt)
        
        # 轉換到 Local 座標
        drone_local = [self.converter.gps_to_local(*pos) for pos in drone_gps_positions]
        target_local = self.converter.gps_to_local(*target_gps)
        
        # 計算隊形
        if mission_type == MissionType.M_FORMATION:
            stage1_local, stage2_local = self._calculate_m_formation(drone_local, target_local, params)
        elif mission_type == MissionType.CIRCLE_FORMATION:
            stage1_local, stage2_local = self._calculate_circle_formation(drone_local, target_local, params)
        elif mission_type == MissionType.LEADER_FOLLOWER:
            stage1_local, stage2_local = self._calculate_leader_follower(drone_local, target_local, params)
        else:
            raise ValueError(f"不支援的任務類型: {mission_type}")
        
        # 轉回 GPS
        stage1_gps = [self.converter.local_to_gps(*pos) for pos in stage1_local]
        stage2_gps = [self.converter.local_to_gps(*pos) for pos in stage2_local]
        
        return stage1_gps, stage2_gps, self.current_origin
    
    def _calculate_m_formation(self, drone_positions, target_point, params):
        """M字形編隊（原本的邏輯）"""
        params = params or {}
        N = len(drone_positions)
        spacing = params.get('spacing', self.spacing)
        base_altitude = params.get('base_altitude', self.base_altitude)
        altitude_diff = params.get('altitude_diff', self.altitude_diff)
        
        C_x = sum(pos[0] for pos in drone_positions) / N
        C_y = sum(pos[1] for pos in drone_positions) / N
        C_z = sum(pos[2] for pos in drone_positions) / N
        
        T_x, T_y, T_z = target_point
        V_x, V_y = T_x - C_x, T_y - C_y
        P_x, P_y = -V_y, V_x
        
        length = math.sqrt(P_x**2 + P_y**2)
        P_x_unit, P_y_unit = (P_x / length, P_y / length) if length > 0.01 else (1.0, 0.0)
        
        projections = [((pos[0] - C_x) * P_x_unit + (pos[1] - C_y) * P_y_unit, i) 
                       for i, pos in enumerate(drone_positions)]
        projections.sort()
        
        stage1_positions = [None] * N
        stage2_positions = [None] * N
        
        for rank, (_, original_idx) in enumerate(projections):
            offset = (rank - (N - 1) / 2) * spacing
            altitude = base_altitude + (altitude_diff if rank % 2 == 0 else -altitude_diff)
            
            stage1_positions[original_idx] = (C_x + P_x_unit * offset, C_y + P_y_unit * offset, altitude)
            stage2_positions[original_idx] = (T_x + P_x_unit * offset, T_y + P_y_unit * offset, altitude)
        
        return stage1_positions, stage2_positions
    
    def _calculate_circle_formation(self, drone_positions, target_point, params):
        """
        圓形編隊
        params: radius(10.0), altitude(10.0), start_angle(0.0)
        """
        params = params or {}
        N = len(drone_positions)
        radius = params.get('radius', 10.0)
        altitude = params.get('altitude', 10.0)
        start_angle = params.get('start_angle', 0.0)
        
        center_x, center_y, center_z = target_point
        stage1_positions = []
        stage2_positions = []
        angle_step = 360.0 / N
        
        for i in range(N):
            angle_rad = math.radians(start_angle + angle_step * i)
            final_x = center_x + radius * math.cos(angle_rad)
            final_y = center_y + radius * math.sin(angle_rad)
            final_z = altitude
            
            current_x, current_y, current_z = drone_positions[i]
            stage1_positions.append((
                current_x + (final_x - current_x) * 0.5,
                current_y + (final_y - current_y) * 0.5,
                current_z + (final_z - current_z) * 0.5
            ))
            stage2_positions.append((final_x, final_y, final_z))
        
        return stage1_positions, stage2_positions
    
    def _calculate_leader_follower(self, drone_positions, target_point, params):
        """
        Leader-Follower 編隊
        params: follow_distance(15.0), altitude_offset(1.0)
        """
        params = params or {}
        N = len(drone_positions)
        follow_distance = params.get('follow_distance', 15.0)  # 預設 15m
        altitude_offset = params.get('altitude_offset', 1.0)
        
        leader_x, leader_y, leader_z = drone_positions[0]
        target_x, target_y, target_z = target_point
        
        direction_x = target_x - leader_x
        direction_y = target_y - leader_y
        direction_length = math.sqrt(direction_x**2 + direction_y**2)
        
        dir_unit_x, dir_unit_y = (direction_x / direction_length, direction_y / direction_length) \
                                  if direction_length > 0.01 else (0.0, 1.0)
        
        stage1_positions = []
        stage2_positions = []
        
        # Leader
        leader_stage1 = (leader_x + direction_x * 0.3, leader_y + direction_y * 0.3, target_z)
        stage1_positions.append(leader_stage1)
        stage2_positions.append(target_point)
        
        # Followers
        for i in range(1, N):
            back_offset = follow_distance * i
            height_offset = altitude_offset * (i % 2)
            
            current_x, current_y, current_z = drone_positions[i]
            follow1_x = leader_stage1[0] - dir_unit_x * back_offset
            follow1_y = leader_stage1[1] - dir_unit_y * back_offset
            follow1_z = leader_stage1[2] + height_offset
            
            stage1_positions.append((
                current_x + (follow1_x - current_x) * 0.5,
                current_y + (follow1_y - current_y) * 0.5,
                current_z + (follow1_z - current_z) * 0.5
            ))
            
            stage2_positions.append((
                target_x - dir_unit_x * back_offset,
                target_y - dir_unit_y * back_offset,
                target_z + height_offset
            ))
        
        return stage1_positions, stage2_positions


# 測試
if __name__ == "__main__":
    planner = FormationPlanner()
    
    drones = [(24.123450, 120.567800, 100.0), (24.123470, 120.567820, 102.0), (24.123440, 120.567810, 98.0)]
    target = (24.123600, 120.568000, 105.0)
    
    # Leader-Follower (15m 間距)
    s1, s2, origin = planner.plan_formation_mission(drones, target, MissionType.LEADER_FOLLOWER, {'follow_distance': 15.0})
    print("Leader-Follower (15m):")
    for i, pos in enumerate(s2):
        print(f"  {'Leader' if i == 0 else f'Follower {i}'}: {pos[0]:.6f}, {pos[1]:.6f}")