399 lines
16 KiB
Python
399 lines
16 KiB
Python
|
# -*- coding: utf-8 -*-
|
|||
|
|
"""
|
|||
|
|
水中航线（WaterRoute）— 完整版
|
|||
|
|
"""
|
|||
|
|
|
|||
|
|
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 WaterParams
|
|||
|
|
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
|
|||
|
|
|
|||
|
|
|
|||
|
|
# ─────────────────────────────────────────────────────────────────
|
|||
|
|
# 水中直线段
|
|||
|
|
# ─────────────────────────────────────────────────────────────────
|
|||
|
|
|
|||
|
|
@dataclass
|
|||
|
|
class WaterSegment(BaseSegment):
|
|||
|
|
"""水中直线航行段（水面/水下通用）"""
|
|||
|
|
domain: EquipmentDomain = field(default=EquipmentDomain.WATER, repr=False)
|
|||
|
|
kind: SegmentKind = field(default=SegmentKind.STRAIGHT, repr=False)
|
|||
|
|
depth_start: float = 0.0
|
|||
|
|
depth_end: float = 0.0
|
|||
|
|
is_subsurface: bool = False
|
|||
|
|
|
|||
|
|
def query(self, t: float) -> WaterParams:
|
|||
|
|
return _water_seg_interpolate(self, t)
|
|||
|
|
|
|||
|
|
def interpolate(self, ratio: float) -> WaterParams:
|
|||
|
|
t = self.t_start + max(0.0, min(1.0, ratio)) * self.duration
|
|||
|
|
return _water_seg_interpolate(self, t)
|
|||
|
|
|
|||
|
|
def validate(self) -> list:
|
|||
|
|
errors = []
|
|||
|
|
if self.distance < 0:
|
|||
|
|
errors.append("距离不能为负")
|
|||
|
|
if self.duration < 0:
|
|||
|
|
errors.append("时长不能为负")
|
|||
|
|
if self.depth_start < 0 or self.depth_end < 0:
|
|||
|
|
errors.append("深度不能为负")
|
|||
|
|
return errors
|
|||
|
|
|
|||
|
|
@property
|
|||
|
|
def depth_change(self) -> float:
|
|||
|
|
return self.depth_end - self.depth_start
|
|||
|
|
|
|||
|
|
@property
|
|||
|
|
def is_diving(self) -> bool:
|
|||
|
|
return self.depth_change > 1.0
|
|||
|
|
|
|||
|
|
@property
|
|||
|
|
def is_surfacing(self) -> bool:
|
|||
|
|
return self.depth_change < -1.0
|
|||
|
|
|
|||
|
|
|
|||
|
|
def _water_seg_interpolate(seg: WaterSegment, t: float) -> WaterParams:
|
|||
|
|
"""水中直线段内匀加速插值"""
|
|||
|
|
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)
|
|||
|
|
# new_lat, new_lon = _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
|
|||
|
|
depth = seg.depth_start + (seg.depth_end - seg.depth_start) * ratio
|
|||
|
|
|
|||
|
|
return WaterParams(
|
|||
|
|
timestamp = t,
|
|||
|
|
latitude = new_lat,
|
|||
|
|
longitude = new_lon,
|
|||
|
|
altitude = alt,
|
|||
|
|
heading = seg.bearing,
|
|||
|
|
speed = speed,
|
|||
|
|
acceleration = seg.acceleration,
|
|||
|
|
depth = depth,
|
|||
|
|
is_subsurface = seg.is_subsurface,
|
|||
|
|
domain = EquipmentDomain.WATER,
|
|||
|
|
)
|
|||
|
|
|
|||
|
|
|
|||
|
|
# ─────────────────────────────────────────────────────────────────
|
|||
|
|
# 水中回转机动段
|
|||
|
|
# ─────────────────────────────────────────────────────────────────
|
|||
|
|
|
|||
|
|
@dataclass
|
|||
|
|
class WaterTurnSegment(BaseTransition):
|
|||
|
|
"""水中回转机动段（舰艇大半径圆弧转向）"""
|
|||
|
|
turn_radius: float = 500.0
|
|||
|
|
depth: float = 0.0
|
|||
|
|
domain: EquipmentDomain = field(default=EquipmentDomain.WATER, repr=False)
|
|||
|
|
kind: SegmentKind = field(default=SegmentKind.MANEUVER, repr=False)
|
|||
|
|
|
|||
|
|
_center_lat: float = field(default=0.0, init=False, repr=False)
|
|||
|
|
_center_lon: float = field(default=0.0, init=False, repr=False)
|
|||
|
|
_exit_lat: float = field(default=0.0, init=False, repr=False)
|
|||
|
|
_exit_lon: float = field(default=0.0, init=False, repr=False)
|
|||
|
|
|
|||
|
|
def __post_init__(self):
|
|||
|
|
self._compute_geometry()
|
|||
|
|
|
|||
|
|
def _compute_geometry(self):
|
|||
|
|
angle = self.turn_angle
|
|||
|
|
r = self.turn_radius
|
|||
|
|
if self.turn_direction >= 0:
|
|||
|
|
center_bearing = (self.entry_heading + 90) % 360
|
|||
|
|
else:
|
|||
|
|
center_bearing = (self.entry_heading - 90) % 360
|
|||
|
|
|
|||
|
|
self._center_lat, self._center_lon = GeoUtils.offset_position(
|
|||
|
|
self.entry_lat, self.entry_lon, center_bearing, r
|
|||
|
|
)
|
|||
|
|
exit_from_center = (center_bearing + 180 + angle) % 360
|
|||
|
|
self._exit_lat, self._exit_lon = GeoUtils.offset_position(
|
|||
|
|
self._center_lat, self._center_lon,
|
|||
|
|
(exit_from_center + 180) % 360, r,
|
|||
|
|
)
|
|||
|
|
arc_len = abs(math.radians(angle)) * r
|
|||
|
|
self.duration = arc_len / self.speed if self.speed > 1e-6 else 0.0
|
|||
|
|
|
|||
|
|
@property
|
|||
|
|
def exit_lat(self) -> float:
|
|||
|
|
return self._exit_lat
|
|||
|
|
|
|||
|
|
@property
|
|||
|
|
def exit_lon(self) -> float:
|
|||
|
|
return self._exit_lon
|
|||
|
|
|
|||
|
|
def query(self, t: float) -> WaterParams:
|
|||
|
|
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))
|
|||
|
|
|
|||
|
|
swept_angle = self.turn_angle * ratio
|
|||
|
|
if self.turn_direction >= 0:
|
|||
|
|
center_bearing = (self.entry_heading + 90) % 360
|
|||
|
|
else:
|
|||
|
|
center_bearing = (self.entry_heading - 90) % 360
|
|||
|
|
|
|||
|
|
entry_from_center = (center_bearing + 180) % 360
|
|||
|
|
current_from_center = (entry_from_center + swept_angle) % 360
|
|||
|
|
cur_lat, cur_lon = GeoUtils.offset_position(
|
|||
|
|
self._center_lat, self._center_lon,
|
|||
|
|
current_from_center, self.turn_radius,
|
|||
|
|
)
|
|||
|
|
cur_heading = (self.entry_heading + swept_angle) % 360
|
|||
|
|
turn_rate = (self.turn_angle / self.duration
|
|||
|
|
if self.duration > 1e-6 else 0.0)
|
|||
|
|
rudder_angle = min(35.0, abs(turn_rate) * 2) * self.turn_direction
|
|||
|
|
|
|||
|
|
return WaterParams(
|
|||
|
|
timestamp = t,
|
|||
|
|
latitude = cur_lat,
|
|||
|
|
longitude = cur_lon,
|
|||
|
|
altitude = self.altitude,
|
|||
|
|
heading = cur_heading,
|
|||
|
|
speed = self.speed,
|
|||
|
|
depth = self.depth,
|
|||
|
|
rudder_angle = rudder_angle,
|
|||
|
|
domain = EquipmentDomain.WATER,
|
|||
|
|
)
|
|||
|
|
|
|||
|
|
def validate(self) -> list:
|
|||
|
|
errors = []
|
|||
|
|
if self.turn_radius <= 0:
|
|||
|
|
errors.append(f"回转半径必须为正: {self.turn_radius}")
|
|||
|
|
if self.speed <= 0:
|
|||
|
|
errors.append(f"速度必须为正: {self.speed}")
|
|||
|
|
return errors
|
|||
|
|
|
|||
|
|
|
|||
|
|
# ─────────────────────────────────────────────────────────────────
|
|||
|
|
# 水中航线
|
|||
|
|
# ─────────────────────────────────────────────────────────────────
|
|||
|
|
|
|||
|
|
class WaterRoute(BaseRoute):
|
|||
|
|
"""水中装备航线（水面舰艇 / 潜艇 / UUV）"""
|
|||
|
|
|
|||
|
|
def __init__(
|
|||
|
|
self,
|
|||
|
|
start_time: float = 0.0,
|
|||
|
|
default_turn_radius: float = 500.0,
|
|||
|
|
min_turn_radius: float = 100.0,
|
|||
|
|
is_subsurface: bool = False,
|
|||
|
|
parent: QObject = None,
|
|||
|
|
):
|
|||
|
|
super().__init__(start_time=start_time, parent=parent)
|
|||
|
|
self.default_turn_radius = default_turn_radius
|
|||
|
|
self.min_turn_radius = min_turn_radius
|
|||
|
|
self.is_subsurface = is_subsurface
|
|||
|
|
|
|||
|
|
@property
|
|||
|
|
def domain(self) -> EquipmentDomain:
|
|||
|
|
return EquipmentDomain.WATER
|
|||
|
|
|
|||
|
|
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] = [
|
|||
|
|
# _bearing(
|
|||
|
|
# self._waypoints[i].latitude, self._waypoints[i].longitude,
|
|||
|
|
# self._waypoints[i+1].latitude, self._waypoints[i+1].longitude,
|
|||
|
|
# )
|
|||
|
|
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
|
|||
|
|
|
|||
|
|
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
|
|||
|
|
|
|||
|
|
r = max(
|
|||
|
|
getattr(wp_b, "turn_radius", 0.0) or self.default_turn_radius,
|
|||
|
|
self.min_turn_radius,
|
|||
|
|
)
|
|||
|
|
turn_angle = GeoUtils.heading_diff(brg_in, brg_out)
|
|||
|
|
need_turn = i < n - 2 and abs(turn_angle) > 0.5
|
|||
|
|
|
|||
|
|
if need_turn:
|
|||
|
|
tip_dist_m = r * abs(math.tan(math.radians(turn_angle / 2)))
|
|||
|
|
tip_lat, tip_lon = GeoUtils.offset_position(
|
|||
|
|
wp_b.latitude, wp_b.longitude,
|
|||
|
|
(brg_in + 180) % 360, tip_dist_m,
|
|||
|
|
)
|
|||
|
|
seg_total = GeoUtils.haversine( prev_exit_lat, prev_exit_lon,
|
|||
|
|
wp_b.latitude, wp_b.longitude,)
|
|||
|
|
# seg_total = _haversine(
|
|||
|
|
# prev_exit_lat, prev_exit_lon,
|
|||
|
|
# wp_b.latitude, wp_b.longitude,
|
|||
|
|
# )
|
|||
|
|
tip_check = GeoUtils.haversine(
|
|||
|
|
wp_b.latitude, wp_b.longitude, tip_lat, tip_lon,
|
|||
|
|
)
|
|||
|
|
# tip_check = _haversine(
|
|||
|
|
# wp_b.latitude, wp_b.longitude, tip_lat, tip_lon,
|
|||
|
|
# )
|
|||
|
|
if tip_check > seg_total * 0.95:
|
|||
|
|
need_turn = False
|
|||
|
|
tip_lat, tip_lon = wp_b.latitude, wp_b.longitude
|
|||
|
|
else:
|
|||
|
|
tip_lat, tip_lon = wp_b.latitude, wp_b.longitude
|
|||
|
|
|
|||
|
|
seg_dist = GeoUtils.haversine(
|
|||
|
|
prev_exit_lat, prev_exit_lon, tip_lat, tip_lon
|
|||
|
|
)
|
|||
|
|
# seg_dist = _haversine(
|
|||
|
|
# prev_exit_lat, prev_exit_lon, tip_lat, tip_lon
|
|||
|
|
# )
|
|||
|
|
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
|
|||
|
|
depth_a = getattr(wp_a, "depth", 0.0)
|
|||
|
|
depth_b = getattr(wp_b, "depth", 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 = tip_lat,
|
|||
|
|
end_lon = tip_lon,
|
|||
|
|
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,
|
|||
|
|
depth_start = depth_a, depth_end = depth_b,
|
|||
|
|
)
|
|||
|
|
self._segments.append(seg)
|
|||
|
|
t_cursor += duration
|
|||
|
|
|
|||
|
|
if need_turn:
|
|||
|
|
trans = self._make_transition(
|
|||
|
|
entry_lat = tip_lat, entry_lon = tip_lon,
|
|||
|
|
entry_alt = wp_b.altitude,
|
|||
|
|
entry_heading = brg_in, exit_heading = brg_out,
|
|||
|
|
speed = wp_b.speed, altitude = wp_b.altitude,
|
|||
|
|
t_start = t_cursor, turn_radius = r,
|
|||
|
|
depth = depth_b,
|
|||
|
|
)
|
|||
|
|
self._transitions.append(trans)
|
|||
|
|
if trans is not None:
|
|||
|
|
t_cursor += trans.duration
|
|||
|
|
prev_exit_lat = trans.exit_lat
|
|||
|
|
prev_exit_lon = trans.exit_lon
|
|||
|
|
else:
|
|||
|
|
prev_exit_lat = tip_lat
|
|||
|
|
prev_exit_lon = tip_lon
|
|||
|
|
else:
|
|||
|
|
self._transitions.append(None)
|
|||
|
|
prev_exit_lat = tip_lat
|
|||
|
|
prev_exit_lon = tip_lon
|
|||
|
|
|
|||
|
|
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,
|
|||
|
|
depth_start=0.0, depth_end=0.0,
|
|||
|
|
**kwargs,
|
|||
|
|
) -> WaterSegment:
|
|||
|
|
return WaterSegment(
|
|||
|
|
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,
|
|||
|
|
depth_start = depth_start, depth_end = depth_end,
|
|||
|
|
is_subsurface = self.is_subsurface,
|
|||
|
|
)
|
|||
|
|
|
|||
|
|
def _make_transition(
|
|||
|
|
self,
|
|||
|
|
entry_lat, entry_lon, entry_alt,
|
|||
|
|
entry_heading, exit_heading,
|
|||
|
|
speed, altitude, t_start,
|
|||
|
|
turn_radius=500.0, depth=0.0,
|
|||
|
|
**kwargs,
|
|||
|
|
) -> Optional[WaterTurnSegment]:
|
|||
|
|
return WaterTurnSegment(
|
|||
|
|
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_radius = turn_radius, depth = depth,
|
|||
|
|
)
|
|||
|
|
|
|||
|
|
def _interpolate_segment(self, seg: BaseSegment, t: float) -> WaterParams:
|
|||
|
|
return _water_seg_interpolate(seg, t) # type: ignore[arg-type]
|
|||
|
|
|
|||
|
|
def _state_at_start(self) -> WaterParams:
|
|||
|
|
seg = self._segments[0]
|
|||
|
|
return WaterParams(
|
|||
|
|
timestamp = self.t_start,
|
|||
|
|
latitude = seg.start_lat,
|
|||
|
|
longitude = seg.start_lon,
|
|||
|
|
altitude = seg.start_alt,
|
|||
|
|
heading = seg.bearing,
|
|||
|
|
speed = seg.v_start,
|
|||
|
|
depth = seg.depth_start, # type: ignore[attr-defined]
|
|||
|
|
is_subsurface = self.is_subsurface,
|
|||
|
|
domain = EquipmentDomain.WATER,
|
|||
|
|
)
|
|||
|
|
|
|||
|
|
def _state_at_end(self) -> WaterParams:
|
|||
|
|
last_trans = self._transitions[-1] if self._transitions else None
|
|||
|
|
if last_trans is not None:
|
|||
|
|
s = last_trans.query(last_trans.t_end)
|
|||
|
|
s.timestamp = self.t_end
|
|||
|
|
return s
|
|||
|
|
last_seg = self._segments[-1]
|
|||
|
|
return WaterParams(
|
|||
|
|
timestamp = self.t_end,
|
|||
|
|
latitude = last_seg.end_lat,
|
|||
|
|
longitude = last_seg.end_lon,
|
|||
|
|
altitude = last_seg.end_alt,
|
|||
|
|
heading = last_seg.bearing,
|
|||
|
|
speed = last_seg.v_end,
|
|||
|
|
depth = last_seg.depth_end, # type: ignore[attr-defined]
|
|||
|
|
is_subsurface = self.is_subsurface,
|
|||
|
|
domain = EquipmentDomain.WATER,
|
|||
|
|
)
|