# -*- 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