565 lines
21 KiB
Python
565 lines
21 KiB
Python
# -*- coding: utf-8 -*-
|
||
"""
|
||
航线抽象基类（核心）
|
||
─────────────────────────────────────────────────────────────────
|
||
BaseRoute —— 所有具体航线类型的统一抽象基类
|
||
|
||
继承体系：
|
||
BaseRoute（ABC）
|
||
├── AirRoute 空中飞行航线（适配 FlightRoute）
|
||
├── GroundRoute 陆地航线
|
||
└── WaterRoute 水中航线
|
||
|
||
设计原则：
|
||
1. 开闭原则：新增航线类型只需继承 BaseRoute，实现抽象方法
|
||
2. 接口隔离：公共接口定义在基类，特有接口在子类扩展
|
||
3. 里氏替换：所有子类实例均可作为 BaseRoute 使用
|
||
4. PyQt5 兼容：继承 QObject 以支持信号槽体系
|
||
─────────────────────────────────────────────────────────────────
|
||
"""
|
||
|
||
from __future__ import annotations
|
||
|
||
import logging
|
||
import time
|
||
from abc import abstractmethod
|
||
from typing import Iterator, List, Optional, Tuple
|
||
|
||
from PyQt5.QtCore import QObject, pyqtSignal
|
||
|
||
from uas.route.base_params import BaseParams
|
||
from uas.route.base_segment import BaseSegment, BaseTransition
|
||
from uas.route.base_waypoint import BaseWaypoint, EquipmentDomain
|
||
|
||
logger = logging.getLogger(__name__)
|
||
|
||
|
||
# ─────────────────────────────────────────────────────────────────
|
||
# 航线元数据
|
||
# ─────────────────────────────────────────────────────────────────
|
||
|
||
class RouteMetadata:
|
||
"""航线附加元数据（非必须，按需使用）"""
|
||
def __init__(self):
|
||
self.name: str = ""
|
||
self.description: str = ""
|
||
self.author: str = ""
|
||
self.created_at: float = time.time()
|
||
self.tags: list = []
|
||
self.extra: dict = {}
|
||
|
||
|
||
# ─────────────────────────────────────────────────────────────────
|
||
# 航线抽象基类
|
||
# ─────────────────────────────────────────────────────────────────
|
||
|
||
class BaseRoute(QObject):
|
||
"""
|
||
航线公共抽象基类
|
||
|
||
── 内部数据结构（交替排列）────────────────────────────────────
|
||
segments[0] → 直线段 0
|
||
transitions[0] → 过渡段 0（可为 None）
|
||
segments[1] → 直线段 1
|
||
transitions[1] → 过渡段 1（可为 None）
|
||
...
|
||
segments[N-1] → 最后直线段（无后续过渡段）
|
||
|
||
── 时间轴────────────────────────────────────────────────────
|
||
t0─[seg0]─t1─[trans0]─t2─[seg1]─t3─[trans1]─t4─...
|
||
|
||
── 子类必须实现的抽象方法 ────────────────────────────────────
|
||
_rebuild() 重建所有段（核心算法）
|
||
_make_segment(...) 构造具体 Segment 实例
|
||
_make_transition(...) 构造具体 Transition 实例（可返回 None）
|
||
_interpolate_segment(...) 段内插值
|
||
_state_at_start() 起点状态
|
||
_state_at_end() 终点状态
|
||
domain 属性：返回 EquipmentDomain
|
||
"""
|
||
|
||
# ── PyQt5 信号 ────────────────────────────────────────────────
|
||
route_changed = pyqtSignal() # 航线发生变化（重建后）
|
||
waypoint_added = pyqtSignal(object) # 新增路径点（BaseWaypoint）
|
||
route_cleared = pyqtSignal() # 航线被清空
|
||
|
||
def __init__(
|
||
self,
|
||
start_time: float = 0.0,
|
||
parent: QObject = None,
|
||
):
|
||
super().__init__(parent)
|
||
self.start_time: float = start_time
|
||
self.total_duration: float = 0.0
|
||
self.metadata: RouteMetadata = RouteMetadata()
|
||
|
||
# 内部数据（子类通过 _rebuild() 填充）
|
||
self._waypoints: List[BaseWaypoint] = []
|
||
self._segments: List[BaseSegment] = []
|
||
self._transitions: List[Optional[BaseTransition]] = []
|
||
|
||
self.logger = logging.getLogger(self.__class__.__name__)
|
||
|
||
# ── 公共属性（只读视图）──────────────────────────────────────
|
||
|
||
@property
|
||
def waypoints(self) -> List[BaseWaypoint]:
|
||
return list(self._waypoints)
|
||
|
||
@property
|
||
def segments(self) -> List[BaseSegment]:
|
||
return list(self._segments)
|
||
|
||
@property
|
||
def transitions(self) -> List[Optional[BaseTransition]]:
|
||
return list(self._transitions)
|
||
|
||
@property
|
||
def waypoint_count(self) -> int:
|
||
return len(self._waypoints)
|
||
|
||
@property
|
||
def segment_count(self) -> int:
|
||
return len(self._segments)
|
||
|
||
@property
|
||
def t_start(self) -> float:
|
||
return self.start_time
|
||
|
||
@property
|
||
def t_end(self) -> float:
|
||
return self.start_time + self.total_duration
|
||
|
||
@property
|
||
def is_empty(self) -> bool:
|
||
return len(self._segments) == 0
|
||
|
||
@property
|
||
def total_distance(self) -> float:
|
||
"""总水平距离（米）"""
|
||
return sum(s.distance for s in self._segments)
|
||
|
||
@property
|
||
def total_transition_time(self) -> float:
|
||
"""所有过渡段总时长（秒）"""
|
||
return sum(
|
||
t.duration for t in self._transitions if t is not None
|
||
)
|
||
|
||
# ── 抽象属性 ──────────────────────────────────────────────────
|
||
|
||
@property
|
||
@abstractmethod
|
||
def domain(self) -> EquipmentDomain:
|
||
"""返回本航线所属的装备域"""
|
||
...
|
||
|
||
# ── 路径点管理（公共接口）────────────────────────────────────
|
||
|
||
def add_waypoint(self, wp: BaseWaypoint) -> "BaseRoute":
|
||
"""
|
||
添加单个路径点
|
||
:return: self（支持链式调用）
|
||
"""
|
||
self._validate_waypoint_domain(wp)
|
||
self._waypoints.append(wp)
|
||
self.waypoint_added.emit(wp)
|
||
if len(self._waypoints) >= 2:
|
||
self._rebuild()
|
||
self.route_changed.emit()
|
||
self.logger.debug(f"添加路径点: {wp.identifier}")
|
||
return self
|
||
|
||
def add_waypoints(self, wps: List[BaseWaypoint]) -> "BaseRoute":
|
||
"""
|
||
批量添加路径点（一次性重建，效率更高）
|
||
:return: self（支持链式调用）
|
||
"""
|
||
for wp in wps:
|
||
self._validate_waypoint_domain(wp)
|
||
self._waypoints.extend(wps)
|
||
if len(self._waypoints) >= 2:
|
||
self._rebuild()
|
||
self.route_changed.emit()
|
||
self.logger.debug(f"批量添加 {len(wps)} 个路径点")
|
||
return self
|
||
|
||
def load_waypoints(self, wps: List[BaseWaypoint]) -> "BaseRoute":
|
||
"""
|
||
替换全部路径点（先清空再加载）
|
||
:return: self（支持链式调用）
|
||
"""
|
||
self._waypoints = list(wps)
|
||
for wp in self._waypoints:
|
||
self._validate_waypoint_domain(wp)
|
||
if len(self._waypoints) >= 2:
|
||
self._rebuild()
|
||
self.route_changed.emit()
|
||
return self
|
||
|
||
def remove_waypoint(self, index: int) -> "BaseRoute":
|
||
"""
|
||
移除指定索引的路径点
|
||
:return: self（支持链式调用）
|
||
"""
|
||
if 0 <= index < len(self._waypoints):
|
||
removed = self._waypoints.pop(index)
|
||
self.logger.debug(f"移除路径点: {removed.identifier}")
|
||
if len(self._waypoints) >= 2:
|
||
self._rebuild()
|
||
else:
|
||
self._segments.clear()
|
||
self._transitions.clear()
|
||
self.total_duration = 0.0
|
||
self.route_changed.emit()
|
||
return self
|
||
|
||
def replace_waypoint(self, index: int,
|
||
wp: BaseWaypoint) -> "BaseRoute":
|
||
"""
|
||
替换指定索引的路径点
|
||
:return: self（支持链式调用）
|
||
"""
|
||
self._validate_waypoint_domain(wp)
|
||
if 0 <= index < len(self._waypoints):
|
||
self._waypoints[index] = wp
|
||
if len(self._waypoints) >= 2:
|
||
self._rebuild()
|
||
self.route_changed.emit()
|
||
return self
|
||
|
||
def clear(self) -> "BaseRoute":
|
||
"""
|
||
清空航线（路径点 + 所有段）
|
||
:return: self（支持链式调用）
|
||
"""
|
||
self._waypoints.clear()
|
||
self._segments.clear()
|
||
self._transitions.clear()
|
||
self.total_duration = 0.0
|
||
self.route_cleared.emit()
|
||
self.logger.debug("航线已清空")
|
||
return self
|
||
|
||
# ── 查询接口（公共）──────────────────────────────────────────
|
||
|
||
def query(self, t: float) -> BaseParams:
|
||
"""
|
||
查询时刻 t 的装备精确状态
|
||
|
||
时间轴遍历：seg[0]→trans[0]→seg[1]→trans[1]→...→seg[N-1]
|
||
边界处理：t < t_start → 起点状态；t > t_end → 终点状态
|
||
"""
|
||
if self.is_empty:
|
||
raise ValueError(
|
||
f"{self.__class__.__name__}: 航线为空，"
|
||
f"请先添加至少两个路径点"
|
||
)
|
||
|
||
# 边界处理
|
||
if t <= self._segments[0].t_start:
|
||
return self._state_at_start()
|
||
if t >= self.t_end:
|
||
return self._state_at_end()
|
||
|
||
# 遍历所有段
|
||
for i, seg in enumerate(self._segments):
|
||
if seg.contains_time(t):
|
||
return self._interpolate_segment(seg, t)
|
||
if i < len(self._transitions):
|
||
trans = self._transitions[i]
|
||
if trans is not None and trans.contains_time(t):
|
||
return trans.query(t)
|
||
|
||
# 兜底（理论上不会到达）
|
||
return self._state_at_end()
|
||
|
||
def query_range(
|
||
self,
|
||
t_from: float,
|
||
t_to: float,
|
||
dt: float = 1.0,
|
||
) -> List[BaseParams]:
|
||
"""
|
||
查询时间范围内的状态序列
|
||
:param t_from: 起始时刻
|
||
:param t_to: 结束时刻
|
||
:param dt: 采样间隔（秒）
|
||
"""
|
||
results = []
|
||
t = t_from
|
||
while t <= t_to + 1e-9:
|
||
results.append(self.query(t))
|
||
t += dt
|
||
return results
|
||
|
||
def find_segment_at(
|
||
self, t: float
|
||
) -> Tuple[Optional[BaseSegment], Optional[BaseTransition]]:
|
||
"""
|
||
查找 t 时刻所在的段
|
||
:return: (BaseSegment, None) 或 (None, BaseTransition)
|
||
"""
|
||
for i, seg in enumerate(self._segments):
|
||
if seg.contains_time(t):
|
||
return seg, None
|
||
if i < len(self._transitions):
|
||
trans = self._transitions[i]
|
||
if trans is not None and trans.contains_time(t):
|
||
return None, trans
|
||
return None, None
|
||
|
||
def iter_segments(
|
||
self,
|
||
) -> Iterator[Tuple[BaseSegment, Optional[BaseTransition]]]:
|
||
"""
|
||
迭代所有 (直线段, 后续过渡段) 对
|
||
:yields: (BaseSegment, Optional[BaseTransition])
|
||
"""
|
||
for i, seg in enumerate(self._segments):
|
||
trans = (self._transitions[i]
|
||
if i < len(self._transitions) else None)
|
||
yield seg, trans
|
||
|
||
# ── 轨迹生成（公共）──────────────────────────────────────────
|
||
|
||
def generate_trajectory(
|
||
self, dt: float = 1.0
|
||
) -> List[BaseParams]:
|
||
"""
|
||
按均匀时间步长采样完整轨迹
|
||
:param dt: 采样间隔（秒）
|
||
:return: BaseParams 列表（含起点和终点）
|
||
"""
|
||
if self.is_empty:
|
||
return []
|
||
trajectory: List[BaseParams] = []
|
||
t = self.t_start
|
||
while t <= self.t_end + 1e-9:
|
||
trajectory.append(self.query(t))
|
||
t += dt
|
||
# 确保终点被包含
|
||
if trajectory and trajectory[-1].timestamp < self.t_end - 1e-9:
|
||
trajectory.append(self.query(self.t_end))
|
||
return trajectory
|
||
|
||
def generate_trajectory_by_distance(
|
||
self, step_m: float = 1000.0
|
||
) -> List[BaseParams]:
|
||
"""
|
||
按距离步长采样轨迹（适用于地图绘制）
|
||
:param step_m: 采样距离间隔（米）
|
||
"""
|
||
if self.is_empty:
|
||
return []
|
||
trajectory: List[BaseParams] = []
|
||
accumulated_dist: float = 0.0
|
||
next_sample_dist: float = 0.0
|
||
prev_state: Optional[BaseParams] = None
|
||
dt = 0.5
|
||
t = self.t_start
|
||
while t <= self.t_end + 1e-9:
|
||
state = self.query(t)
|
||
if prev_state is not None:
|
||
d = state.distance_to(prev_state)
|
||
accumulated_dist += d
|
||
if accumulated_dist >= next_sample_dist:
|
||
trajectory.append(state)
|
||
next_sample_dist += step_m
|
||
else:
|
||
trajectory.append(state) # 起点
|
||
prev_state = state
|
||
t += dt
|
||
return trajectory
|
||
|
||
# ── 统计与摘要（公共）────────────────────────────────────────
|
||
|
||
def statistics(self) -> dict:
|
||
"""返回航线统计数据字典"""
|
||
if self.is_empty:
|
||
return {}
|
||
segs = self._segments
|
||
return {
|
||
"domain": self.domain.value,
|
||
"waypoint_count": len(self._waypoints),
|
||
"segment_count": len(segs),
|
||
"transition_count": sum(1 for t in self._transitions
|
||
if t is not None),
|
||
"total_distance_km": self.total_distance / 1000,
|
||
"total_duration_s": self.total_duration,
|
||
"total_duration_min": self.total_duration / 60,
|
||
"transition_time_s": self.total_transition_time,
|
||
"climb_distance_km": sum(s.distance for s in segs
|
||
if s.is_climb) / 1000,
|
||
"descent_distance_km": sum(s.distance for s in segs
|
||
if s.is_descent) / 1000,
|
||
"level_distance_km": sum(s.distance for s in segs
|
||
if s.is_level) / 1000,
|
||
"max_altitude_m": max(max(s.start_alt, s.end_alt)
|
||
for s in segs),
|
||
"min_altitude_m": min(min(s.start_alt, s.end_alt)
|
||
for s in segs),
|
||
"avg_speed_ms": (
|
||
sum(s.avg_speed * s.duration for s in segs) /
|
||
sum(s.duration for s in segs)
|
||
if segs else 0.0
|
||
),
|
||
}
|
||
|
||
def info(self) -> str:
|
||
"""返回航线完整摘要字符串"""
|
||
if self.is_empty:
|
||
return f"{self.__class__.__name__}: 航线为空"
|
||
st = self.statistics()
|
||
lines = [
|
||
f"{'═'*65}",
|
||
f" {self.__class__.__name__} 航线概览 [{self.domain.value}]",
|
||
f"{'═'*65}",
|
||
f" 路径点数 : {st['waypoint_count']}",
|
||
f" 直线段数 : {st['segment_count']}",
|
||
f" 过渡段数 : {st['transition_count']}",
|
||
f" 总距离 : {st['total_distance_km']:.3f} km",
|
||
f" 总时长 : {st['total_duration_s']:.1f}s "
|
||
f"({st['total_duration_min']:.2f} min)",
|
||
f" 平均速度 : {st['avg_speed_ms']:.1f} m/s",
|
||
f" 高度范围 : {st['min_altitude_m']:.0f}m ~ "
|
||
f"{st['max_altitude_m']:.0f}m",
|
||
f"{'─'*65}",
|
||
f" {'#':>3} {'起点':>6}→{'终点':<6} "
|
||
f"{'距离(km)':>9} {'方位(°)':>8} "
|
||
f"{'时长(s)':>8} {'速度(m/s)':>12}",
|
||
f"{'─'*65}",
|
||
]
|
||
for i, (seg, trans) in enumerate(self.iter_segments()):
|
||
lines.append(
|
||
f" {i+1:>3} "
|
||
f"{seg.wp_from.identifier:>6}→{seg.wp_to.identifier:<6} "
|
||
f"{seg.distance/1000:>9.3f} "
|
||
f"{seg.bearing:>8.1f} "
|
||
f"{seg.duration:>8.1f} "
|
||
f"{seg.v_start:>5.1f}→{seg.v_end:<5.1f}"
|
||
)
|
||
if trans is not None:
|
||
dir_str = "右转" if trans.turn_direction > 0 else "左转"
|
||
lines.append(
|
||
f" ↩ {dir_str}"
|
||
f"{abs(trans.turn_angle):.1f}° "
|
||
f"({trans.duration:.1f}s)"
|
||
)
|
||
lines.append(f"{'═'*65}")
|
||
return "\n".join(lines)
|
||
|
||
def validate(self) -> list:
|
||
"""
|
||
校验整条航线合法性
|
||
:return: 错误信息列表（空列表=合法）
|
||
"""
|
||
errors = []
|
||
if len(self._waypoints) < 2:
|
||
errors.append("路径点数量不足（至少需要2个）")
|
||
for i, wp in enumerate(self._waypoints):
|
||
wp_errors = wp.validate()
|
||
for e in wp_errors:
|
||
errors.append(f"路径点[{i}] {wp.identifier}: {e}")
|
||
for i, seg in enumerate(self._segments):
|
||
seg_errors = seg.validate()
|
||
for e in seg_errors:
|
||
errors.append(f"航段[{i}]: {e}")
|
||
return errors
|
||
|
||
# ── 抽象方法（子类必须实现）──────────────────────────────────
|
||
|
||
@abstractmethod
|
||
def _rebuild(self) -> None:
|
||
"""
|
||
重建所有直线段和过渡段（核心算法）
|
||
|
||
实现要求：
|
||
1. 清空 _segments 和 _transitions
|
||
2. 遍历 _waypoints，逐段构建 BaseSegment
|
||
3. 在需要转弯处插入 BaseTransition
|
||
4. 更新 total_duration
|
||
"""
|
||
...
|
||
|
||
@abstractmethod
|
||
def _make_segment(
|
||
self,
|
||
wp_from: BaseWaypoint,
|
||
wp_to: BaseWaypoint,
|
||
start_lat: float, start_lon: float, start_alt: float,
|
||
end_lat: float, end_lon: float, end_alt: float,
|
||
bearing: float,
|
||
t_start: float,
|
||
) -> BaseSegment:
|
||
"""
|
||
构造具体 Segment 实例（工厂方法）
|
||
子类返回对应域的 Segment 子类实例
|
||
"""
|
||
...
|
||
|
||
@abstractmethod
|
||
def _make_transition(
|
||
self,
|
||
entry_lat: float, entry_lon: float, entry_alt: float,
|
||
entry_heading: float, exit_heading: float,
|
||
speed: float, altitude: float,
|
||
t_start: float,
|
||
) -> Optional[BaseTransition]:
|
||
"""
|
||
构造具体 Transition 实例（工厂方法）
|
||
子类返回对应域的 Transition 子类实例，
|
||
若不需要过渡段则返回 None
|
||
"""
|
||
...
|
||
|
||
@abstractmethod
|
||
def _interpolate_segment(
|
||
self, seg: BaseSegment, t: float
|
||
) -> BaseParams:
|
||
"""
|
||
在直线段内对 t 时刻进行插值
|
||
子类实现具体的运动学模型（匀速/匀加速等）
|
||
"""
|
||
...
|
||
|
||
@abstractmethod
|
||
def _state_at_start(self) -> BaseParams:
|
||
"""返回航线起点状态"""
|
||
...
|
||
|
||
@abstractmethod
|
||
def _state_at_end(self) -> BaseParams:
|
||
"""返回航线终点状态"""
|
||
...
|
||
|
||
# ── 内部工具 ──────────────────────────────────────────────────
|
||
|
||
def _validate_waypoint_domain(self, wp: BaseWaypoint) -> None:
|
||
"""
|
||
校验路径点域与航线域是否匹配
|
||
（宽松检查：domain=AIR 的基类 WP 可用于任何航线）
|
||
"""
|
||
pass # 子类可覆盖以实现严格检查
|
||
|
||
def _update_total_duration(self) -> None:
|
||
"""根据所有段重新计算 total_duration"""
|
||
if not self._segments:
|
||
self.total_duration = 0.0
|
||
return
|
||
seg_time = sum(s.duration for s in self._segments)
|
||
trans_time = sum(
|
||
t.duration for t in self._transitions if t is not None
|
||
)
|
||
self.total_duration = seg_time + trans_time
|
||
|
||
def __repr__(self) -> str:
|
||
return (f"{self.__class__.__name__}("
|
||
f"domain={self.domain.value}, "
|
||
f"wps={len(self._waypoints)}, "
|
||
f"segs={len(self._segments)}, "
|
||
f"dist={self.total_distance/1000:.1f}km, "
|
||
f"dur={self.total_duration:.0f}s)")
|
||
|
||
class FlightRoute(BaseRoute):
|
||
pass |