skyeyesystem/backend/Skyeye-sys-dev/skyeye-service-py/klkx/planning/uav/uav.py

from enum import StrEnum
import numpy as np
from shapely.geometry import Point

from planning.beidou import LocationCode
from planning.config import FRONT_DISTANCE

class PayloadType(StrEnum):
    optics_down = "optics_down"
    optics_front = "optics_front"
    sar = "SAR"
    laser = "laser"

class Uav():
    
    id: int
    uav_type: str
    start_coord: Point
    start_code: str
    end_coord: Point
    end_code: str
    
    # 通用
    height: float # 飞行高度(m)
    endurance: float # 续航里程(m)
    length: float # x 方向幅宽(m)
    width: float # y 方向幅宽(m)
    
    # 光学
    valid_length: float # x 方向有效幅宽(m)
    valid_width: float # y 方向有效幅宽(m)
    start_height: float = 0 # 起飞椭球高度(m)
    end_height: float = 0 # 降落椭球高度(m)
    
    # SAR 
    theta: float # 下视角(deg)
    deviation_distance: float # 偏移距离(m)
    reserved_distance: float # 预留距离(m)
    direction: int # 推扫方向
    
    def __init__(
        self, 
        id: int, 
        start_coord: Point, 
        end_coord: Point,
        payload: dict, 
        constraint: dict
        ):
        """
        初始化无人机类
            
        Args:
            id(int): 无人机ID
            start_coord(NDArray): 无人机起始经纬度坐标(deg)
            end_coord(NDArray): 无人机结束经纬度坐标(deg)
            endurance(SupportsFloat): 无人机续航里程(m)
        """
        
        self.id = id
        self.start_coord = start_coord
        self.start_code = LocationCode.to_code(
            lon = start_coord.x,
            lat = start_coord.y,
            level = 10)
        self.end_coord = end_coord
        self.end_code = LocationCode.to_code(
            lon = end_coord.x,
            lat = end_coord.y,
            level = 10)
        
        self.endurance = constraint["endurance"]
        self.height = constraint.get("height")
        self.resolution = constraint.get("resolution")
        self.x_overlap = constraint["x_overlap"] if "x_overlap" in constraint else 0
        self.y_overlap = constraint["y_overlap"] if "y_overlap" in constraint else 0
        
        if payload["type"] == PayloadType.optics_down:
            
            sensor_size = payload["sensor_size"] * 0.0254
            sensor_width = sensor_size * 0.6
            sensor_length = sensor_size * 0.8
            focus = payload["equivalent_focus"] / 43.27 * sensor_size 
            
            if self.height is not None:
                self.resolution = sensor_width / payload["pixel_width"] * self.height / focus
                # self.y_resolution = payload["sensorh"] / payload["ph"] * self.height / payload["focus"]
            elif self.resolution is not None:
                self.height = self.resolution / sensor_width * payload["pixel_width"] * focus
            else:
                raise ValueError("The \"height\" and \"resolution\" must have one that is not None.")
            self.length = self.resolution * payload["pixel_width"]
            self.width = self.resolution *payload["pixel_height"]
            
        elif payload["type"] == PayloadType.optics_front:
                
            if "start_height" in payload:
                self.start_height = payload["start_height"]
            if "end_height" in payload:
                self.end_height = payload["end_height"]
                
            sensor_size = payload["sensor_size"] * 0.0254
            sensor_width = sensor_size * 0.6
            sensor_length = sensor_size * 0.8
            focus = payload["equivalent_focus"] / 43.27 * sensor_size 
            
            # 相似三角形推算地面实际覆盖
            self.width = (sensor_width * FRONT_DISTANCE) / focus
            self.length = (sensor_length * FRONT_DISTANCE) / focus
        
        elif payload["type"] == PayloadType.sar:
            if self.height < 0 or self.height > 500:
                raise ValueError("The height must be more than 0m and less than 500m.")
            
            self.theta = payload["theta"]
            if self.theta < 0 or self.theta > 90:
                raise ValueError("The theta must be more than 0 degree and less than 90 degree.")
            
            # self.length = payload["length"]
            self.width = payload["width"]
            self.reserved_distance = payload["reserved_distance"]
            self.deviation_distance = self.height * np.tan(np.radians(self.theta))
            self.direction = payload["direction"]
            # if payload["direction"] == "left":
            #     self.direction = 1
            # elif payload["direction"] == "right":
            #     self.direction = -1
            # else:
            #     raise ValueError("The direction must be left or right.")
            self.length = self.height / np.cos(np.radians(self.theta)) * 26 / 180 * np.pi / 2
            
        elif payload["type"] == PayloadType.laser:
            self.length = 0
            self.width = 0
            
        self.valid_length = self.length * (1 - self.x_overlap)
        self.valid_width = self.width * (1 - self.y_overlap)