432 lines
16 KiB
Python
432 lines
16 KiB
Python
# -*- 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.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 = GeoUtils.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,
|
||
) |