# -*- coding: utf-8 -*- """ 陆地航线（GroundRoute） ───────────────────────────────────────────────────────────────── 继承 BaseRoute，实现地面车辆/步兵装备的路径表示。 特有特性： - 高度跟随地形（无飞行高度概念） - 转弯段为原地/低速转弯（非圆弧） - 支持停车点（dwell_time > 0） - 支持坡度计算 ───────────────────────────────────────────────────────────────── """ from __future__ import annotations import math from dataclasses import dataclass, field from typing import List, Optional from PyQt5.QtCore import QObject from uas.route.base_params import GroundParams from uas.route.base_route import BaseRoute from uas.route.base_segment import BaseSegment, BaseTransition, SegmentKind from uas.route.base_waypoint import EquipmentDomain from uas.utils import geo_utils from uas.utils.geo_utils import GeoUtils # 复用工具函数 # from uas.route.routes.route.air_route import _haversine, _bearing, _offset_position, _heading_diff # ───────────────────────────────────────────────────────────────── # 陆地直线段 # ───────────────────────────────────────────────────────────────── @dataclass class GroundSegment(BaseSegment): """陆地直线行驶段""" domain: EquipmentDomain = field(default=EquipmentDomain.GROUND, repr=False) kind: SegmentKind = field(default=SegmentKind.STRAIGHT, repr=False) road_type: str = "road" # road / offroad / track slope_deg: float = 0.0 # 平均坡度（度，上坡为正） def query(self, t: float) -> GroundParams: return _ground_seg_interpolate(self, t) def interpolate(self, ratio: float) -> GroundParams: t = self.t_start + max(0.0, min(1.0, ratio)) * self.duration return _ground_seg_interpolate(self, t) def validate(self) -> list: errors = [] if self.distance < 0: errors.append("距离不能为负") if self.duration < 0: errors.append("时长不能为负") if self.v_start < 0 or self.v_end < 0: errors.append("速度不能为负") if abs(self.slope_deg) > 90: errors.append(f"坡度角越界: {self.slope_deg}") return errors @property def odometer_start(self) -> float: """里程计起始值（由路由累计，默认0）""" return self.metadata.get("odometer_start", 0.0) def _ground_seg_interpolate(seg: GroundSegment, t: float) -> GroundParams: """陆地直线段内匀加速插值""" dt = t - seg.t_start ratio = dt / seg.duration if seg.duration > 1e-6 else 1.0 ratio = max(0.0, min(1.0, ratio)) dist = seg.v_start * dt + 0.5 * seg.acceleration * dt * dt dist = max(0.0, min(dist, seg.distance)) new_lat, new_lon = geo_utils.offset_position( seg.start_lat, seg.start_lon, seg.bearing, dist ) speed = max(seg.v_start + seg.acceleration * dt, 0.0) alt = seg.start_alt + (seg.end_alt - seg.start_alt) * ratio # 里程计 odometer = seg.odometer_start + dist return GroundParams( timestamp = t, latitude = new_lat, longitude = new_lon, altitude = alt, heading = seg.bearing, speed = speed, acceleration = seg.acceleration, slope_deg = seg.slope_deg, road_type = seg.road_type, is_stopped = speed < 0.1, odometer_m = odometer, domain = EquipmentDomain.GROUND, ) # ───────────────────────────────────────────────────────────────── # 陆地转弯段（原地/低速转弯） # ───────────────────────────────────────────────────────────────── @dataclass class GroundTurnSegment(BaseTransition): """ 陆地转弯段 车辆在路径点处减速转向，转弯期间位置基本不变（原地转向） 或以小半径低速转弯。 """ turn_speed: float = 2.0 # 转弯速度（m/s，默认低速） domain: EquipmentDomain = field( default=EquipmentDomain.GROUND, repr=False ) kind: SegmentKind = field( default=SegmentKind.TURN, repr=False ) def __post_init__(self): # 转弯时长 = 转弯角度 / 转弯角速度（默认 45°/s） turn_rate_deg_s = 45.0 self.duration = abs(self.turn_angle) / turn_rate_deg_s @property def exit_lat(self) -> float: # 地面转弯：位置变化极小，近似为入口位置 return self.entry_lat @property def exit_lon(self) -> float: return self.entry_lon def query(self, t: float) -> GroundParams: dt = t - self.t_start ratio = dt / self.duration if self.duration > 1e-6 else 1.0 ratio = max(0.0, min(1.0, ratio)) # 当前航向：线性插值 cur_heading = ( self.entry_heading + self.turn_angle * ratio ) % 360 return GroundParams( timestamp = t, latitude = self.entry_lat, longitude = self.entry_lon, altitude = self.altitude, heading = cur_heading, speed = self.turn_speed, is_stopped = self.turn_speed < 0.1, domain = EquipmentDomain.GROUND, ) def validate(self) -> list: errors = [] if self.turn_speed < 0: errors.append("转弯速度不能为负") return errors # ───────────────────────────────────────────────────────────────── # 陆地停车段（dwell） # ───────────────────────────────────────────────────────────────── @dataclass class GroundDwellSegment(BaseTransition): """ 陆地停车段 装备在路径点处停留 dwell_time 秒。 """ dwell_time: float = 0.0 domain: EquipmentDomain = field( default=EquipmentDomain.GROUND, repr=False ) kind: SegmentKind = field( default=SegmentKind.HOLD, repr=False ) def __post_init__(self): self.duration = self.dwell_time @property def exit_lat(self) -> float: return self.entry_lat @property def exit_lon(self) -> float: return self.entry_lon def query(self, t: float) -> GroundParams: return GroundParams( timestamp = t, latitude = self.entry_lat, longitude = self.entry_lon, altitude = self.altitude, heading = self.entry_heading, speed = 0.0, is_stopped = True, domain = EquipmentDomain.GROUND, ) def validate(self) -> list: return [] if self.dwell_time >= 0 else ["停留时间不能为负"] # ───────────────────────────────────────────────────────────────── # 陆地航线 # ───────────────────────────────────────────────────────────────── class GroundRoute(BaseRoute): """ 陆地装备航线 特性： - 支持停车点（dwell_time > 0 的路径点自动插入停车段） - 支持转弯段（原地低速转弯） - 坡度自动从相邻路径点高度差计算 """ def __init__( self, start_time: float = 0.0, default_turn_speed: float = 2.0, parent: QObject = None, ): super().__init__(start_time=start_time, parent=parent) self.default_turn_speed = default_turn_speed @property def domain(self) -> EquipmentDomain: return EquipmentDomain.GROUND # ── 核心重建算法 ────────────────────────────────────────────── def _rebuild(self) -> None: self._segments.clear() self._transitions.clear() n = len(self._waypoints) if n < 2: self.total_duration = 0.0 return # 预计算方位角 bearings: List[float] = [ GeoUtils.bearing( self._waypoints[i].latitude, self._waypoints[i].longitude, self._waypoints[i+1].latitude, self._waypoints[i+1].longitude, ) for i in range(n - 1) ] t_cursor = self.start_time prev_exit_lat = self._waypoints[0].latitude prev_exit_lon = self._waypoints[0].longitude odometer = 0.0 for i in range(n - 1): wp_a = self._waypoints[i] wp_b = self._waypoints[i + 1] brg_in = bearings[i] brg_out = bearings[i + 1] if (i + 1) < len(bearings) else brg_in # 构建直线段 seg_dist = GeoUtils.haversine( prev_exit_lat, prev_exit_lon, wp_b.latitude, wp_b.longitude, ) v0 = wp_a.speed v1 = wp_b.speed v_avg = (v0 + v1) / 2.0 or 1.0 duration = seg_dist / v_avg if v_avg > 1e-6 else 0.0 acc = (v1 - v0) / duration if duration > 1e-6 else 0.0 # 坡度 h_diff = wp_b.altitude - wp_a.altitude slope = (math.degrees(math.atan2(h_diff, seg_dist)) if seg_dist > 1e-6 else 0.0) seg = self._make_segment( wp_from = wp_a, wp_to = wp_b, start_lat = prev_exit_lat, start_lon = prev_exit_lon, start_alt = wp_a.altitude, end_lat = wp_b.latitude, end_lon = wp_b.longitude, end_alt = wp_b.altitude, bearing = brg_in, t_start = t_cursor, v_start = v0, v_end = v1, acc = acc, dist = seg_dist, duration = duration, slope_deg = slope, odometer_start = odometer, ) self._segments.append(seg) t_cursor += duration odometer += seg_dist # 停车段（dwell_time > 0） dwell = getattr(wp_b, "dwell_time", 0.0) if dwell > 0: dwell_seg = GroundDwellSegment( entry_lat = wp_b.latitude, entry_lon = wp_b.longitude, entry_alt = wp_b.altitude, entry_heading = brg_in, exit_heading = brg_out, speed = 0.0, altitude = wp_b.altitude, t_start = t_cursor, dwell_time = dwell, ) self._transitions.append(dwell_seg) t_cursor += dwell_seg.duration else: # 转弯段 turn_angle = GeoUtils.heading_diff(brg_in, brg_out) need_turn = i < n - 2 and abs(turn_angle) > 1.0 if need_turn: trans = self._make_transition( entry_lat = wp_b.latitude, entry_lon = wp_b.longitude, entry_alt = wp_b.altitude, entry_heading = brg_in, exit_heading = brg_out, speed = self.default_turn_speed, altitude = wp_b.altitude, t_start = t_cursor, ) self._transitions.append(trans) if trans is not None: t_cursor += trans.duration else: self._transitions.append(None) prev_exit_lat = wp_b.latitude prev_exit_lon = wp_b.longitude self._update_total_duration() # ── 工厂方法 ────────────────────────────────────────────────── def _make_segment( self, wp_from, wp_to, start_lat, start_lon, start_alt, end_lat, end_lon, end_alt, bearing, t_start, v_start=0.0, v_end=0.0, acc=0.0, dist=0.0, duration=0.0, slope_deg=0.0, odometer_start=0.0, **kwargs, ) -> GroundSegment: seg = GroundSegment( wp_from = wp_from, wp_to = wp_to, start_lat = start_lat, start_lon = start_lon, start_alt = start_alt, end_lat = end_lat, end_lon = end_lon, end_alt = end_alt, distance = dist, bearing = bearing, t_start = t_start, t_end = t_start + duration, duration = duration, v_start = v_start, v_end = v_end, acceleration = acc, slope_deg = slope_deg, ) seg.metadata["odometer_start"] = odometer_start return seg def _make_transition( self, entry_lat, entry_lon, entry_alt, entry_heading, exit_heading, speed, altitude, t_start, **kwargs, ) -> Optional[GroundTurnSegment]: return GroundTurnSegment( entry_lat = entry_lat, entry_lon = entry_lon, entry_alt = entry_alt, entry_heading = entry_heading, exit_heading = exit_heading, speed = speed, altitude = altitude, t_start = t_start, turn_speed = self.default_turn_speed, ) def _interpolate_segment( self, seg: BaseSegment, t: float ) -> GroundParams: return _ground_seg_interpolate(seg, t) # type: ignore[arg-type] def _state_at_start(self) -> GroundParams: seg = self._segments[0] return GroundParams( timestamp = self.t_start, latitude = seg.start_lat, longitude = seg.start_lon, altitude = seg.start_alt, heading = seg.bearing, speed = seg.v_start, slope_deg = seg.slope_deg, # type: ignore[attr-defined] domain = EquipmentDomain.GROUND, ) def _state_at_end(self) -> GroundParams: last_seg = self._segments[-1] return GroundParams( timestamp = self.t_end, latitude = last_seg.end_lat, longitude = last_seg.end_lon, altitude = last_seg.end_alt, heading = last_seg.bearing, speed = 0.0, is_stopped = True, domain = EquipmentDomain.GROUND, )