liusongtao
/
base_agent
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity
base_agent/uas/route/routes/ground_route.py

432 lines
16 KiB
Python
Raw Normal View History

in devel
2026-06-18 03:28:14 +00:00
# -*- 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))
in devel
2026-06-18 03:50:33 +00:00
new_lat, new_lon = GeoUtils.offset_position(
in devel
2026-06-18 03:28:14 +00:00
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,
)