433 lines
16 KiB
Python
433 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 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,
|
|||
|
|
)
|