268 lines
12 KiB
Python
268 lines
12 KiB
Python
# -*- coding: utf-8 -*-
|
||
"""
|
||
航段抽象基类
|
||
─────────────────────────────────────────────────────────────────
|
||
BaseSegment —— 所有直线/曲线航段的公共抽象基类
|
||
BaseTransition —— 两段之间过渡段（转弯/机动）的抽象基类
|
||
|
||
继承体系：
|
||
BaseSegment
|
||
├── AirSegment 空中直线段（适配 RouteSegment）
|
||
├── GroundSegment 陆地直线段
|
||
└── WaterSegment 水中直线段
|
||
|
||
BaseTransition
|
||
├── AirTurnSegment 空中转弯圆弧段（适配 TurnSegment）
|
||
├── GroundTurnSegment 陆地转弯段
|
||
└── WaterTurnSegment 水中机动段
|
||
─────────────────────────────────────────────────────────────────
|
||
"""
|
||
|
||
from __future__ import annotations
|
||
|
||
from abc import ABC, abstractmethod
|
||
from dataclasses import dataclass, field
|
||
from enum import Enum
|
||
from typing import Any, Dict, TYPE_CHECKING
|
||
|
||
from uas.route.base_params import BaseParams
|
||
from uas.route.base_waypoint import BaseWaypoint, EquipmentDomain
|
||
|
||
if TYPE_CHECKING:
|
||
pass
|
||
|
||
|
||
# ─────────────────────────────────────────────────────────────────
|
||
# 航段类型枚举
|
||
# ─────────────────────────────────────────────────────────────────
|
||
|
||
class SegmentKind(Enum):
|
||
STRAIGHT = "straight" # 直线段
|
||
TURN = "turn" # 转弯/圆弧段
|
||
HOLD = "hold" # 等待/盘旋段
|
||
APPROACH = "approach" # 进近段
|
||
MANEUVER = "maneuver" # 机动段（水下/地面）
|
||
CUSTOM = "custom" # 自定义
|
||
|
||
|
||
# ─────────────────────────────────────────────────────────────────
|
||
# 航段抽象基类
|
||
# ─────────────────────────────────────────────────────────────────
|
||
|
||
@dataclass
|
||
class BaseSegment(ABC):
|
||
"""
|
||
航段公共抽象基类
|
||
|
||
每段代表航线上一个连续的运动片段，
|
||
包含起止坐标、时间区间、速度/方位等核心参数。
|
||
|
||
子类必须实现：
|
||
query(t) —— 查询段内 t 时刻的装备状态
|
||
interpolate(ratio)—— 按比例插值（0.0=起点，1.0=终点）
|
||
validate() —— 校验航段合法性
|
||
"""
|
||
|
||
# ── 逻辑路径点引用 ────────────────────────────────────────────
|
||
wp_from: BaseWaypoint # 起始逻辑路径点
|
||
wp_to: BaseWaypoint # 终止逻辑路径点
|
||
|
||
# ── 实际飞行起终点坐标 ────────────────────────────────────────
|
||
start_lat: float # 实际起点纬度（度）
|
||
start_lon: float # 实际起点经度（度）
|
||
start_alt: float # 实际起点高度（米）
|
||
end_lat: float # 实际终点纬度（度）
|
||
end_lon: float # 实际终点经度（度）
|
||
end_alt: float # 实际终点高度（米）
|
||
|
||
# ── 几何参数 ──────────────────────────────────────────────────
|
||
distance: float # 水平距离（米）
|
||
bearing: float # 初始方位角（度，真北顺时针）
|
||
|
||
# ── 时间参数 ──────────────────────────────────────────────────
|
||
t_start: float # 航段起始时刻（秒）
|
||
t_end: float # 航段结束时刻（秒）
|
||
duration: float # 飞行时长（秒）
|
||
|
||
# ── 速度参数 ──────────────────────────────────────────────────
|
||
v_start: float # 起点速度（m/s）
|
||
v_end: float # 终点速度（m/s）
|
||
acceleration: float = 0.0 # 匀加速度（m/s²）
|
||
|
||
# ── 域与类型 ──────────────────────────────────────────────────
|
||
domain: EquipmentDomain = EquipmentDomain.AIR
|
||
kind: SegmentKind = SegmentKind.STRAIGHT
|
||
|
||
# ── 扩展元数据 ────────────────────────────────────────────────
|
||
metadata: Dict[str, Any] = field(default_factory=dict)
|
||
|
||
# ── 衍生属性 ──────────────────────────────────────────────────
|
||
|
||
@property
|
||
def avg_speed(self) -> float:
|
||
return (self.v_start + self.v_end) / 2.0
|
||
|
||
@property
|
||
def vertical_distance(self) -> float:
|
||
return self.end_alt - self.start_alt
|
||
|
||
@property
|
||
def vertical_speed(self) -> float:
|
||
if self.duration < 1e-6:
|
||
return 0.0
|
||
return self.vertical_distance / self.duration
|
||
|
||
@property
|
||
def is_climb(self) -> bool:
|
||
return self.vertical_distance > 10
|
||
|
||
@property
|
||
def is_descent(self) -> bool:
|
||
return self.vertical_distance < -10
|
||
|
||
@property
|
||
def is_level(self) -> bool:
|
||
return abs(self.vertical_distance) <= 10
|
||
|
||
@property
|
||
def start_position(self) -> tuple:
|
||
return (self.start_lat, self.start_lon, self.start_alt)
|
||
|
||
@property
|
||
def end_position(self) -> tuple:
|
||
return (self.end_lat, self.end_lon, self.end_alt)
|
||
|
||
def contains_time(self, t: float) -> bool:
|
||
"""判断时刻 t 是否落在本段时间范围内"""
|
||
return self.t_start <= t <= self.t_end
|
||
|
||
# ── 抽象接口 ──────────────────────────────────────────────────
|
||
|
||
@abstractmethod
|
||
def query(self, t: float) -> BaseParams:
|
||
"""
|
||
查询段内 t 时刻的装备状态
|
||
:param t: 查询时刻（秒）
|
||
:return: BaseParams 或其子类实例
|
||
"""
|
||
...
|
||
|
||
@abstractmethod
|
||
def interpolate(self, ratio: float) -> BaseParams:
|
||
"""
|
||
按比例插值
|
||
:param ratio: 0.0=起点，1.0=终点
|
||
:return: BaseParams 或其子类实例
|
||
"""
|
||
...
|
||
|
||
@abstractmethod
|
||
def validate(self) -> list:
|
||
"""
|
||
校验航段合法性
|
||
:return: 错误信息列表（空列表=合法）
|
||
"""
|
||
...
|
||
|
||
def __str__(self) -> str:
|
||
phase = "爬升" if self.is_climb else ("下降" if self.is_descent else "平飞")
|
||
return (f"{self.__class__.__name__}["
|
||
f"{self.wp_from.identifier}→{self.wp_to.identifier}] "
|
||
f"{self.distance/1000:.2f}km/{self.bearing:.1f}°/"
|
||
f"{self.duration:.1f}s/{phase}]")
|
||
|
||
|
||
# ─────────────────────────────────────────────────────────────────
|
||
# 过渡段抽象基类（转弯/机动）
|
||
# ─────────────────────────────────────────────────────────────────
|
||
|
||
@dataclass
|
||
class BaseTransition(ABC):
|
||
"""
|
||
两段之间过渡段的公共抽象基类
|
||
|
||
过渡段连接两个直线段，描述转弯、机动等非直线运动。
|
||
典型实例：
|
||
AirTurnSegment —— 飞机 Fly-by 圆弧转弯
|
||
GroundTurnSegment—— 车辆转弯段
|
||
WaterTurnSegment —— 舰艇回转机动
|
||
|
||
子类必须实现 query(t) 和 validate()。
|
||
"""
|
||
|
||
# ── 入口/出口参数 ─────────────────────────────────────────────
|
||
entry_lat: float # 入口纬度（度）
|
||
entry_lon: float # 入口经度（度）
|
||
entry_alt: float # 入口高度（米）
|
||
entry_heading: float # 入口航向（度）
|
||
exit_heading: float # 出口航向（度）
|
||
|
||
# ── 时间参数 ──────────────────────────────────────────────────
|
||
t_start: float # 过渡段起始时刻（秒）
|
||
duration: float = 0.0 # 过渡段时长（秒，子类计算后填充）
|
||
|
||
# ── 运动参数 ──────────────────────────────────────────────────
|
||
speed: float = 0.0 # 过渡段速度（m/s）
|
||
altitude: float = 0.0 # 过渡段高度（米）
|
||
|
||
# ── 域与类型 ──────────────────────────────────────────────────
|
||
domain: EquipmentDomain = EquipmentDomain.AIR
|
||
kind: SegmentKind = SegmentKind.TURN
|
||
|
||
# ── 扩展元数据 ────────────────────────────────────────────────
|
||
metadata: Dict[str, Any] = field(default_factory=dict)
|
||
|
||
# ── 衍生属性 ──────────────────────────────────────────────────
|
||
|
||
@property
|
||
def t_end(self) -> float:
|
||
return self.t_start + self.duration
|
||
|
||
@property
|
||
def turn_angle(self) -> float:
|
||
"""转弯角（度，右转为正，左转为负）"""
|
||
diff = self.exit_heading - self.entry_heading
|
||
while diff > 180: diff -= 360
|
||
while diff < -180: diff += 360
|
||
return diff
|
||
|
||
@property
|
||
def turn_direction(self) -> int:
|
||
"""转弯方向：+1=右转，-1=左转，0=直线"""
|
||
a = self.turn_angle
|
||
if a > 0.5: return 1
|
||
elif a < -0.5: return -1
|
||
return 0
|
||
|
||
def contains_time(self, t: float) -> bool:
|
||
return self.t_start <= t <= self.t_end
|
||
|
||
# ── 抽象接口 ──────────────────────────────────────────────────
|
||
|
||
@abstractmethod
|
||
def query(self, t: float) -> BaseParams:
|
||
"""查询过渡段内 t 时刻的装备状态"""
|
||
...
|
||
|
||
@abstractmethod
|
||
def validate(self) -> list:
|
||
"""校验过渡段合法性"""
|
||
...
|
||
|
||
@property
|
||
@abstractmethod
|
||
def exit_lat(self) -> float:
|
||
"""出口纬度（子类计算）"""
|
||
...
|
||
|
||
@property
|
||
@abstractmethod
|
||
def exit_lon(self) -> float:
|
||
"""出口经度（子类计算）"""
|
||
...
|
||
|
||
def __str__(self) -> str:
|
||
dir_str = "右转" if self.turn_direction > 0 else "左转"
|
||
return (f"{self.__class__.__name__}["
|
||
f"{dir_str}{abs(self.turn_angle):.1f}° "
|
||
f"t={self.t_start:.1f}~{self.t_end:.1f}s]") |