843 lines
30 KiB
Python
843 lines
30 KiB
Python
#!/usr/bin/env python3
|
||
"""
|
||
PX4 SITL Offboard 视觉跟踪集成测试 (MAVSDK-Python)
|
||
连接: udpin://0.0.0.0:14540
|
||
流程:
|
||
解锁 → 起飞 → 切换 Offboard（速度设定点）
|
||
→ 视觉跟踪闭环 20 Hz（追踪器 → 控制律 → offboard 网关）
|
||
→ 目标消失 → Coast 预测 → 判丢零速刹车 → 退出 Offboard → RTL → 落地 → 上锁
|
||
适用于 PX4 1.17
|
||
|
||
YOLO 检测未接入：VirtualTarget 用无人机真实位置/航向做针孔反投影生成
|
||
检测框与激光测距（几何一致闭环），无人机会实际飞向该虚拟目标。
|
||
"""
|
||
|
||
import asyncio
|
||
import logging
|
||
import math
|
||
import sys
|
||
import time
|
||
from pathlib import Path
|
||
|
||
import matplotlib
|
||
import matplotlib.pyplot as plt
|
||
from matplotlib import font_manager
|
||
from matplotlib.ticker import MaxNLocator
|
||
|
||
|
||
def _setup_chinese_font() -> str | None:
|
||
"""为 matplotlib 配置可用的中文字体，避免中文标题/图例乱码。
|
||
|
||
优先从字体文件注册，再按 family 名称匹配（Linux Noto/WQY、
|
||
Windows 雅黑/黑体、macOS 苹方等）。返回实际字体名；失败返回 None。
|
||
"""
|
||
# 1) 直接按路径注册（TTC/TTF）
|
||
path_candidates = [
|
||
"/usr/share/fonts/opentype/noto/NotoSansCJK-Regular.ttc",
|
||
"/usr/share/fonts/opentype/noto/NotoSerifCJK-Regular.ttc",
|
||
"/usr/share/fonts/truetype/droid/DroidSansFallbackFull.ttf",
|
||
"/usr/share/fonts/truetype/wqy/wqy-microhei.ttc",
|
||
"/usr/share/fonts/truetype/wqy/wqy-zenhei.ttc",
|
||
"C:/Windows/Fonts/msyh.ttc",
|
||
"C:/Windows/Fonts/msyhbd.ttc",
|
||
"C:/Windows/Fonts/simhei.ttf",
|
||
"/System/Library/Fonts/PingFang.ttc",
|
||
"/System/Library/Fonts/STHeiti Light.ttc",
|
||
]
|
||
chosen = None
|
||
for path in path_candidates:
|
||
if not Path(path).is_file():
|
||
continue
|
||
try:
|
||
font_manager.fontManager.addfont(path)
|
||
prop = font_manager.FontProperties(fname=path)
|
||
chosen = prop.get_name()
|
||
break
|
||
except Exception:
|
||
continue
|
||
|
||
# 2) 按已注册 family 名称匹配
|
||
if chosen is None:
|
||
name_candidates = [
|
||
"Noto Sans CJK SC",
|
||
"Noto Sans CJK JP",
|
||
"Noto Serif CJK SC",
|
||
"Noto Serif CJK JP",
|
||
"Droid Sans Fallback",
|
||
"WenQuanYi Micro Hei",
|
||
"WenQuanYi Zen Hei",
|
||
"Source Han Sans SC",
|
||
"Source Han Sans CN",
|
||
"Microsoft YaHei",
|
||
"SimHei",
|
||
"PingFang SC",
|
||
"Heiti SC",
|
||
"Arial Unicode MS",
|
||
]
|
||
available = {f.name for f in font_manager.fontManager.ttflist}
|
||
for name in name_candidates:
|
||
if name in available:
|
||
chosen = name
|
||
break
|
||
|
||
if chosen:
|
||
matplotlib.rcParams["font.sans-serif"] = [chosen, "DejaVu Sans"]
|
||
matplotlib.rcParams["font.family"] = "sans-serif"
|
||
# 解决坐标轴负号显示为方块的问题
|
||
matplotlib.rcParams["axes.unicode_minus"] = False
|
||
return chosen
|
||
|
||
|
||
_CHINESE_FONT = _setup_chinese_font()
|
||
|
||
_UTILS_DIR = Path(__file__).resolve().parent / "utils"
|
||
if str(_UTILS_DIR) not in sys.path:
|
||
sys.path.insert(0, str(_UTILS_DIR))
|
||
|
||
from mavsdk import System
|
||
from mavsdk.action import ActionError
|
||
|
||
from utils.target_tracker import Detection, FrameShape, TargetTracker, TrackerConfig
|
||
from utils.demo_track_control import track_result_to_state
|
||
from utils.visual_track_control.config import load_from_json
|
||
from utils.visual_track_control.mavsdk_gateway import MavsdkCommandGateway
|
||
from utils.visual_track_control.tracking_control_law import TrackingControlLaw
|
||
from utils.visual_track_control.types import ControlMode, ControlOutput, ControlSnapshot
|
||
|
||
logger = logging.getLogger("test")
|
||
|
||
# ==================== 配置 ====================
|
||
CONNECTION = "udpin://0.0.0.0:14540" # 推荐写法，也可用 "udp://127.0.0.1:14540"
|
||
TAKEOFF_ALTITUDE = 5.0 # 起飞高度 (m)
|
||
|
||
TRACK_CONFIG_PATH = _UTILS_DIR / "config" / "track_control.json"
|
||
TRACK_MAX_LOST_FRAMES = 60 # 追踪器 Coast 窗口（20 Hz 下 = 3 s）
|
||
TRACK_NORMAL_SECS = 20.0 # 虚拟目标保持可见时长（之后进入丢失场景）
|
||
TRACK_BRAKE_SECS = 5.0 # 判丢后零速刹车时长
|
||
TRACK_TIMEOUT_SECS = 120.0 # 跟踪段整体安全超时
|
||
STATUS_EVERY_TICKS = 10 # 每 0.5 s 打印一行状态
|
||
PLOT_TRAJECTORY = False # 跟踪结束后绘制无人机与虚拟目标轨迹
|
||
TRAJECTORY_SAVE_PATH = Path(__file__).resolve().parent / "trajectory_plot.png"
|
||
|
||
# 虚拟目标：相对悬停锚点、按锚点机头朝向放置的机体系偏移
|
||
# （前方 AHEAD / 右方 RIGHT，横偏用于激发航向通道）。
|
||
# 默认与悬停点同高：地面目标场景下垂直通道会持续下降趋近目标高度，
|
||
# SITL 中有触发 PX4 着陆检测的风险；TARGET_DOWN_M 取正值（目标更低，
|
||
# 如 +5.0 = 地面）即可切换为地面目标场景。
|
||
TARGET_AHEAD_M = 30.0
|
||
TARGET_RIGHT_M = 15.0
|
||
TARGET_DOWN_M = 0.0
|
||
MOCK_BOX_W = 120.0 # 模拟检测框尺寸 (px)
|
||
MOCK_BOX_H = 160.0
|
||
MOCK_CONFIDENCE = 0.9
|
||
MOCK_TARGET_SPEED_MPS = 1.0 # 圆弧切向速度 (m/s)
|
||
TARGET_ARC_RADIUS_M = 12.0 # 圆弧半径 (m)
|
||
# 旋转方向：+1 = NED 水平面逆时针（从北向东转），-1 = 顺时针
|
||
TARGET_ARC_CCW = 1
|
||
|
||
|
||
def now_ms() -> int:
|
||
return int(time.monotonic() * 1000.0)
|
||
|
||
|
||
def plot_trajectories(
|
||
drone_traj: list,
|
||
target_traj: list,
|
||
save_path=None,
|
||
) -> None:
|
||
"""绘制无人机与虚拟目标的 NED 轨迹。
|
||
|
||
参数:
|
||
drone_traj / target_traj: 列表元素为 (t_s, north_m, east_m, down_m)
|
||
save_path: 若给定则保存图片，否则仅弹窗显示
|
||
"""
|
||
if not drone_traj and not target_traj:
|
||
print("! 无轨迹数据，跳过绘图")
|
||
return
|
||
|
||
if _CHINESE_FONT is None:
|
||
print("! 未找到中文字体，图中中文可能显示为方框。"
|
||
"可安装 fonts-noto-cjk 或文泉驿，或改用英文标签。")
|
||
else:
|
||
# 再次确保本进程内 rc 生效（部分后端会重置）
|
||
matplotlib.rcParams["font.sans-serif"] = [_CHINESE_FONT, "DejaVu Sans"]
|
||
matplotlib.rcParams["axes.unicode_minus"] = False
|
||
|
||
fig, axes = plt.subplots(1, 2, figsize=(12, 5))
|
||
|
||
# ---- 左图：水平面 N-E 轨迹（俯视图）----
|
||
ax = axes[0]
|
||
if drone_traj:
|
||
dn = [p[1] for p in drone_traj]
|
||
de = [p[2] for p in drone_traj]
|
||
ax.plot(de, dn, "b-", linewidth=1.5, label="无人机")
|
||
ax.scatter(de[0], dn[0], c="blue", s=60, marker="o", zorder=5, label="无人机起点")
|
||
ax.scatter(de[-1], dn[-1], c="blue", s=80, marker="x", zorder=5, label="无人机终点")
|
||
if target_traj:
|
||
tn = [p[1] for p in target_traj]
|
||
te = [p[2] for p in target_traj]
|
||
ax.plot(te, tn, "r--", linewidth=1.5, label="虚拟目标")
|
||
ax.scatter(te[0], tn[0], c="red", s=60, marker="o", zorder=5, label="目标起点")
|
||
ax.scatter(te[-1], tn[-1], c="red", s=80, marker="x", zorder=5, label="目标终点")
|
||
ax.set_xlabel("East (m)")
|
||
ax.set_ylabel("North (m)")
|
||
ax.set_title("水平轨迹 (NED 俯视)")
|
||
ax.set_aspect("equal", adjustable="datalim")
|
||
ax.grid(True, linestyle="--", alpha=0.6)
|
||
ax.legend(loc="best", fontsize=8)
|
||
ax.xaxis.set_major_locator(MaxNLocator(nbins=6))
|
||
ax.yaxis.set_major_locator(MaxNLocator(nbins=6))
|
||
|
||
# ---- 右图：高度随时间（Down 取负号显示为高度）----
|
||
ax2 = axes[1]
|
||
if drone_traj:
|
||
dt = [p[0] for p in drone_traj]
|
||
d_alt = [-p[3] for p in drone_traj] # NED down → 高度
|
||
ax2.plot(dt, d_alt, "b-", linewidth=1.5, label="无人机高度")
|
||
if target_traj:
|
||
tt = [p[0] for p in target_traj]
|
||
t_alt = [-p[3] for p in target_traj]
|
||
ax2.plot(tt, t_alt, "r--", linewidth=1.5, label="目标高度")
|
||
ax2.set_xlabel("时间 (s)")
|
||
ax2.set_ylabel("高度 (m)")
|
||
ax2.set_title("高度-时间曲线")
|
||
ax2.grid(True, linestyle="--", alpha=0.6)
|
||
ax2.legend(loc="best", fontsize=8)
|
||
|
||
fig.suptitle("视觉跟踪段轨迹", fontsize=12)
|
||
fig.tight_layout()
|
||
|
||
if save_path is not None:
|
||
fig.savefig(str(save_path), dpi=150, bbox_inches="tight")
|
||
print(f"-- 轨迹图已保存: {save_path}")
|
||
try:
|
||
plt.show()
|
||
except Exception as e:
|
||
print(f"! 无法弹窗显示图像（无图形界面？）: {e}")
|
||
finally:
|
||
plt.close(fig)
|
||
|
||
|
||
class VirtualTarget:
|
||
"""几何一致虚拟目标 —— YOLO 检测与激光雷达的替身。
|
||
|
||
目标初始位置：
|
||
以起飞后的悬停锚点为参考，
|
||
按起飞时机头方向放置：
|
||
前方 TARGET_AHEAD_M
|
||
右方 TARGET_RIGHT_M
|
||
下方 TARGET_DOWN_M
|
||
|
||
目标运动（水平圆弧）：
|
||
跟踪开始时刻 t=0，目标以切向速度 MOCK_TARGET_SPEED_MPS、
|
||
半径 TARGET_ARC_RADIUS_M 在 NED 水平面内做匀速圆周运动。
|
||
|
||
圆心：初始位置沿“起飞时右方”偏移半径 R（目标从圆心左侧/右侧
|
||
起转，初始切向近似沿机头前方），高度保持不变。
|
||
|
||
角速度 ω = v / R，方向由 TARGET_ARC_CCW 控制（+1 逆时针）。
|
||
|
||
每次 observe() 调用时，根据经过的时间实时更新目标的
|
||
NED 世界坐标，然后再进行针孔投影。
|
||
|
||
相机假定与机体固连、无视差、忽略 roll/pitch；
|
||
仅使用 yaw 将世界 NED 坐标转换到机体系。
|
||
"""
|
||
|
||
def __init__(
|
||
self,
|
||
cfg,
|
||
anchor_ned: tuple[float, float, float],
|
||
anchor_yaw_rad: float,
|
||
) -> None:
|
||
|
||
# ============================================================
|
||
# 1. 根据起飞后的锚点 + 起飞时航向，计算目标初始位置
|
||
# ============================================================
|
||
|
||
cos_y = math.cos(anchor_yaw_rad)
|
||
sin_y = math.sin(anchor_yaw_rad)
|
||
|
||
# 机体系：
|
||
# 前方 = (cos(yaw), sin(yaw))
|
||
# 右方 = (-sin(yaw), cos(yaw))
|
||
|
||
self._start_n = (
|
||
anchor_ned[0]
|
||
+ TARGET_AHEAD_M * cos_y
|
||
- TARGET_RIGHT_M * sin_y
|
||
)
|
||
|
||
self._start_e = (
|
||
anchor_ned[1]
|
||
+ TARGET_AHEAD_M * sin_y
|
||
+ TARGET_RIGHT_M * cos_y
|
||
)
|
||
|
||
self._start_d = (
|
||
anchor_ned[2]
|
||
+ TARGET_DOWN_M
|
||
)
|
||
|
||
# ============================================================
|
||
# 2. 当前目标位置
|
||
# ============================================================
|
||
|
||
self._pos_n = self._start_n
|
||
self._pos_e = self._start_e
|
||
self._pos_d = self._start_d
|
||
|
||
# ============================================================
|
||
# 3. 圆弧参数
|
||
#
|
||
# 圆心取在初始点沿“起飞时右方”偏移 R 处，使 t=0 时目标
|
||
# 位于圆心左侧（相对机头），初始切向大致沿机头前方：
|
||
#
|
||
# 右方单位向量: (-sin(yaw), cos(yaw))
|
||
# 圆心 = start + R * 右方
|
||
# 初始相位：从圆心指向 start 的向量 = -R * 右方
|
||
# ============================================================
|
||
|
||
self._radius_m = max(0.1, float(TARGET_ARC_RADIUS_M))
|
||
self._omega = (
|
||
float(MOCK_TARGET_SPEED_MPS) / self._radius_m
|
||
) * (1.0 if TARGET_ARC_CCW >= 0 else -1.0)
|
||
|
||
right_n = -sin_y
|
||
right_e = cos_y
|
||
|
||
self._center_n = self._start_n + self._radius_m * right_n
|
||
self._center_e = self._start_e + self._radius_m * right_e
|
||
|
||
# 初始角度：圆心 → 起点 在 NE 平面的方位角
|
||
# atan2(E, N) 与常见航向定义一致
|
||
self._theta0 = math.atan2(
|
||
self._start_e - self._center_e,
|
||
self._start_n - self._center_n,
|
||
)
|
||
|
||
self._move_yaw_rad = anchor_yaw_rad # 仅作参考/日志
|
||
|
||
# ============================================================
|
||
# 4. 记录目标运动开始时间
|
||
# ============================================================
|
||
|
||
self._start_time = time.monotonic()
|
||
|
||
# ============================================================
|
||
# 5. 相机参数
|
||
# ============================================================
|
||
|
||
cam = cfg.camera
|
||
|
||
self._img_w = float(cam.image_width)
|
||
self._img_h = float(cam.image_height)
|
||
|
||
# 针孔模型焦距（像素）：
|
||
#
|
||
# f = W / (2 * tan(FOV_h / 2))
|
||
#
|
||
self._f_px = self._img_w / (
|
||
2.0 * math.tan(
|
||
math.radians(cam.fov_h_deg) / 2.0
|
||
)
|
||
)
|
||
|
||
# ============================================================
|
||
# 6. 目标可见状态
|
||
# ============================================================
|
||
|
||
self._visible = True
|
||
|
||
@property
|
||
def visible(self) -> bool:
|
||
"""返回目标当前是否可见。"""
|
||
return self._visible
|
||
|
||
def set_visible(self, visible: bool) -> None:
|
||
"""设置目标是否可见。"""
|
||
self._visible = visible
|
||
|
||
def update_position(self) -> None:
|
||
"""根据经过的时间更新目标世界坐标（水平匀速圆弧）。
|
||
|
||
θ(t) = θ0 + ω * t
|
||
N = center_N + R * cos(θ)
|
||
E = center_E + R * sin(θ)
|
||
D 保持初始高度不变。
|
||
|
||
切向速度 v = |ω| * R = MOCK_TARGET_SPEED_MPS。
|
||
"""
|
||
|
||
elapsed_s = time.monotonic() - self._start_time
|
||
elapsed_s = max(0.0, elapsed_s)
|
||
|
||
theta = self._theta0 + self._omega * elapsed_s
|
||
|
||
self._pos_n = self._center_n + self._radius_m * math.cos(theta)
|
||
self._pos_e = self._center_e + self._radius_m * math.sin(theta)
|
||
self._pos_d = self._start_d
|
||
|
||
def observe(
|
||
self,
|
||
ned_n: float,
|
||
ned_e: float,
|
||
ned_d: float,
|
||
yaw_rad: float,
|
||
):
|
||
"""返回：
|
||
|
||
(detections, radar_distance_m, associated)
|
||
|
||
不可见时：
|
||
|
||
([], 0.0, False)
|
||
|
||
每次调用都会先更新虚拟目标位置，
|
||
然后根据无人机当前位置和当前 yaw
|
||
计算目标在相机画面中的投影。
|
||
"""
|
||
|
||
# ============================================================
|
||
# 1. 始终更新目标真实世界坐标（即使“不可见”也继续运动，
|
||
# 便于轨迹记录；仅检测输出被屏蔽）
|
||
# ============================================================
|
||
|
||
self.update_position()
|
||
|
||
# ============================================================
|
||
# 2. 目标不可见 → 不返回检测框
|
||
# ============================================================
|
||
|
||
if not self._visible:
|
||
return [], 0.0, False
|
||
|
||
# ============================================================
|
||
# 3. 目标相对于无人机的 NED 坐标
|
||
# ============================================================
|
||
|
||
rel_n = self._pos_n - ned_n
|
||
rel_e = self._pos_e - ned_e
|
||
rel_d = self._pos_d - ned_d
|
||
|
||
# ============================================================
|
||
# 4. 世界坐标 NED → 当前无人机机体系
|
||
#
|
||
# 这里只考虑 yaw，不考虑 roll / pitch。
|
||
#
|
||
# fwd ：目标在机头前方的距离
|
||
# right：目标在机体右侧的距离
|
||
# ============================================================
|
||
|
||
cos_y = math.cos(yaw_rad)
|
||
sin_y = math.sin(yaw_rad)
|
||
|
||
fwd = (
|
||
rel_n * cos_y
|
||
+ rel_e * sin_y
|
||
)
|
||
|
||
right = (
|
||
-rel_n * sin_y
|
||
+ rel_e * cos_y
|
||
)
|
||
|
||
# ============================================================
|
||
# 5. 目标在侧面或后方
|
||
# ============================================================
|
||
|
||
if fwd <= 0.5:
|
||
return [], 0.0, False
|
||
|
||
# ============================================================
|
||
# 6. 针孔相机投影
|
||
# ============================================================
|
||
|
||
u = (
|
||
self._img_w * 0.5
|
||
+ self._f_px * (right / fwd)
|
||
)
|
||
|
||
v = (
|
||
self._img_h * 0.5
|
||
+ self._f_px * (rel_d / fwd)
|
||
)
|
||
|
||
# ============================================================
|
||
# 7. 目标投影超出画面
|
||
# ============================================================
|
||
|
||
if not (
|
||
0.0 <= u < self._img_w
|
||
and
|
||
0.0 <= v < self._img_h
|
||
):
|
||
return [], 0.0, False
|
||
|
||
# ============================================================
|
||
# 8. 构造模拟 YOLO 检测框
|
||
# ============================================================
|
||
|
||
box = Detection(
|
||
u - MOCK_BOX_W / 2.0,
|
||
v - MOCK_BOX_H / 2.0,
|
||
u + MOCK_BOX_W / 2.0,
|
||
v + MOCK_BOX_H / 2.0,
|
||
MOCK_CONFIDENCE,
|
||
)
|
||
|
||
# ============================================================
|
||
# 9. 计算模拟激光雷达距离
|
||
# ============================================================
|
||
|
||
slant = math.sqrt(
|
||
fwd * fwd
|
||
+ right * right
|
||
+ rel_d * rel_d
|
||
)
|
||
|
||
return [box], slant, True
|
||
|
||
|
||
class TelemetryHub:
|
||
"""后台订阅姿态/位置遥测，控制回路每拍读取最新快照（§7.1 同拍取样）。"""
|
||
|
||
def __init__(self) -> None:
|
||
self.yaw_rad = 0.0
|
||
self.roll_rad = 0.0
|
||
self.pitch_rad = 0.0
|
||
self.attitude_time_ms = 0
|
||
self.north_m = 0.0
|
||
self.east_m = 0.0
|
||
self.down_m = 0.0
|
||
self._tasks: list[asyncio.Task] = []
|
||
|
||
def start(self, drone) -> None:
|
||
self._tasks.append(asyncio.create_task(self._attitude_loop(drone)))
|
||
self._tasks.append(asyncio.create_task(self._position_loop(drone)))
|
||
|
||
async def _attitude_loop(self, drone) -> None:
|
||
async for euler in drone.telemetry.attitude_euler():
|
||
self.yaw_rad = math.radians(euler.yaw_deg)
|
||
self.roll_rad = math.radians(euler.roll_deg)
|
||
self.pitch_rad = math.radians(euler.pitch_deg)
|
||
self.attitude_time_ms = now_ms()
|
||
|
||
async def _position_loop(self, drone) -> None:
|
||
async for pos_vel in drone.telemetry.position_velocity_ned():
|
||
pos = pos_vel.position
|
||
self.north_m = pos.north_m
|
||
self.east_m = pos.east_m
|
||
self.down_m = pos.down_m
|
||
|
||
async def wait_attitude(self, timeout_s: float = 10.0) -> bool:
|
||
deadline = time.monotonic() + timeout_s
|
||
while self.attitude_time_ms == 0 and time.monotonic() < deadline:
|
||
await asyncio.sleep(0.1)
|
||
return self.attitude_time_ms > 0
|
||
|
||
async def stop(self) -> None:
|
||
for task in self._tasks:
|
||
task.cancel()
|
||
for task in self._tasks:
|
||
try:
|
||
await task
|
||
except asyncio.CancelledError:
|
||
pass
|
||
self._tasks.clear()
|
||
|
||
|
||
async def dispatch(sender, out: ControlOutput, tick_ms: int) -> None:
|
||
"""按模式分发指令。
|
||
|
||
DEGRADED_HOLD 在胶水层 demo 中不发送，但真实 offboard 会话中断流
|
||
（约 0.5~1 s）会触发 PX4 offboard failsafe，故此处改发零速设定点
|
||
（等效刹停悬停）保住设定点流。
|
||
"""
|
||
if out.mode is ControlMode.VELOCITY_HEADING and out.valid:
|
||
await sender.send_velocity_heading(out, tick_ms)
|
||
else:
|
||
await sender.send_brake_hover(tick_ms)
|
||
|
||
|
||
async def run_tracking(drone, anchor_ned: tuple[float, float, float],
|
||
anchor_yaw_deg: float) -> bool:
|
||
"""视觉跟踪闭环段：20 Hz 控制回路，返回是否正常走完场景。"""
|
||
cfg = load_from_json(str(TRACK_CONFIG_PATH))
|
||
period_s = 1.0 / cfg.control.frequency_hz
|
||
|
||
hub = TelemetryHub()
|
||
hub.start(drone)
|
||
if not await hub.wait_attitude():
|
||
logger.error("姿态遥测未就绪，放弃跟踪段")
|
||
await hub.stop()
|
||
return False
|
||
|
||
tracker = TargetTracker(TrackerConfig(max_lost_frames=TRACK_MAX_LOST_FRAMES))
|
||
law = TrackingControlLaw(cfg)
|
||
gateway = MavsdkCommandGateway(drone)
|
||
target = VirtualTarget(cfg, anchor_ned, math.radians(anchor_yaw_deg))
|
||
frame_shape = FrameShape(height=cfg.camera.image_height,
|
||
width=cfg.camera.image_width)
|
||
|
||
if not await gateway.start():
|
||
logger.error("offboard 启动失败，放弃跟踪段")
|
||
await hub.stop()
|
||
return False
|
||
|
||
print(f"-- 进入视觉跟踪：锚点航向 {anchor_yaw_deg:.1f}°，虚拟目标按机头朝向放置于 "
|
||
f"前 {TARGET_AHEAD_M:.0f} m / 右 {TARGET_RIGHT_M:.0f} m / 下 {TARGET_DOWN_M:.0f} m "
|
||
f"(世界 NED: N={target._pos_n:.1f}, E={target._pos_e:.1f}, D={target._pos_d:.1f})")
|
||
print(f" 目标运动: 圆弧 R={TARGET_ARC_RADIUS_M:.1f} m, v={MOCK_TARGET_SPEED_MPS:.1f} m/s, "
|
||
f"{'逆时针' if TARGET_ARC_CCW >= 0 else '顺时针'} "
|
||
f"(圆心 N={target._center_n:.1f}, E={target._center_e:.1f})")
|
||
print(f" 场景: 正常跟踪 {TRACK_NORMAL_SECS:.0f}s → 目标消失 → "
|
||
f"Coast {TRACK_MAX_LOST_FRAMES / cfg.control.frequency_hz:.0f}s → "
|
||
f"判丢刹车 {TRACK_BRAKE_SECS:.0f}s")
|
||
|
||
started_ms = now_ms()
|
||
target_gone_ms = started_ms + TRACK_NORMAL_SECS * 1000.0
|
||
lost_at_ms = None
|
||
next_deadline = time.monotonic() + period_s
|
||
tick = 0
|
||
|
||
# 轨迹记录：(t_s, north_m, east_m, down_m)
|
||
drone_traj = []
|
||
target_traj = []
|
||
|
||
try:
|
||
while True:
|
||
now = now_ms()
|
||
if now - started_ms >= TRACK_TIMEOUT_SECS * 1000.0:
|
||
logger.warning("跟踪段超时（%.0f s），提前收尾", TRACK_TIMEOUT_SECS)
|
||
break
|
||
|
||
if target.visible and now >= target_gone_ms:
|
||
target.set_visible(False)
|
||
print("-- 模拟目标消失，追踪器进入 Coast 预测")
|
||
|
||
detections, radar_m, associated = target.observe(
|
||
hub.north_m, hub.east_m, hub.down_m, hub.yaw_rad)
|
||
|
||
# 记录本拍位置（目标位置在 observe 内已 update）
|
||
t_s = (now - started_ms) / 1000.0
|
||
drone_traj.append((t_s, hub.north_m, hub.east_m, hub.down_m))
|
||
target_traj.append((t_s, target._pos_n, target._pos_e, target._pos_d))
|
||
|
||
result = tracker.update(detections, frame_shape)
|
||
snap = ControlSnapshot(
|
||
track=track_result_to_state(result),
|
||
track_time_ms=now, # 每拍取样即打戳（视觉始终新鲜）
|
||
radar_distance_m=radar_m,
|
||
associated_to_target=associated,
|
||
radar_time_ms=now if associated else 0,
|
||
attitude_present=True,
|
||
attitude_time_ms=hub.attitude_time_ms,
|
||
yaw_rad=hub.yaw_rad,
|
||
roll_rad=hub.roll_rad,
|
||
pitch_rad=hub.pitch_rad,
|
||
)
|
||
out = law.update(snap, now)
|
||
await dispatch(gateway, out, now)
|
||
|
||
if tick % STATUS_EVERY_TICKS == 0:
|
||
print(f"[跟踪 t={t_s:5.1f}s] "
|
||
f"检测={len(detections)} "
|
||
f"追踪[trk={result.tracked} pred={result.is_predicted} "
|
||
f"lost={result.lost_frames}] 雷达={radar_m:5.1f}m | "
|
||
f"[{out.mode.name}] vx_body={out.vx_body_mps:+.2f} "
|
||
f"vz={out.vz_mps:+.2f} yaw_rate={out.yaw_rate_dps:+.2f}°/s "
|
||
f"coast={out.coast_active}")
|
||
tick += 1
|
||
|
||
if not result.tracked:
|
||
if lost_at_ms is None:
|
||
lost_at_ms = now
|
||
print("-- 判丢，执行零速刹车")
|
||
elif now - lost_at_ms >= TRACK_BRAKE_SECS * 1000.0:
|
||
print("-- 判丢刹车完成，退出跟踪段")
|
||
break
|
||
|
||
next_deadline += period_s
|
||
sleep_s = next_deadline - time.monotonic()
|
||
if sleep_s > 0:
|
||
await asyncio.sleep(sleep_s)
|
||
else:
|
||
next_deadline = time.monotonic() + period_s # 节拍落后，重锚
|
||
finally:
|
||
await gateway.stop()
|
||
await hub.stop()
|
||
|
||
# 跟踪段结束后绘制轨迹
|
||
if PLOT_TRAJECTORY:
|
||
print(f"-- 绘制轨迹：无人机 {len(drone_traj)} 点，目标 {len(target_traj)} 点")
|
||
plot_trajectories(drone_traj, target_traj, TRAJECTORY_SAVE_PATH)
|
||
|
||
return True
|
||
|
||
|
||
async def print_flight_mode(drone):
|
||
"""打印当前飞行模式"""
|
||
async for flight_mode in drone.telemetry.flight_mode():
|
||
print(f"[模式] 当前飞行模式: {flight_mode}")
|
||
return flight_mode
|
||
|
||
async def wait_until_ready(drone):
|
||
"""等待连接 + 位置估计就绪（各阶段限时 30 s）。"""
|
||
print("等待无人机连接...")
|
||
try:
|
||
async with asyncio.timeout(30.0):
|
||
async for state in drone.core.connection_state():
|
||
if state.is_connected:
|
||
print("-- 已连接")
|
||
break
|
||
except TimeoutError:
|
||
raise RuntimeError("30 s 内未建立 MAVSDK 连接（检查 SITL 是否在运行、"
|
||
"14540 端口是否被占用）") from None
|
||
|
||
print("等待全球位置估计就绪...")
|
||
try:
|
||
async with asyncio.timeout(30.0):
|
||
async for health in drone.telemetry.health():
|
||
if health.is_global_position_ok and health.is_home_position_ok:
|
||
print("-- 位置估计 OK")
|
||
break
|
||
except TimeoutError:
|
||
raise RuntimeError("30 s 内位置估计未就绪") from None
|
||
|
||
async def run():
|
||
drone = System()
|
||
await drone.connect(system_address=CONNECTION)
|
||
|
||
# 1. 等待就绪
|
||
try:
|
||
await wait_until_ready(drone)
|
||
except RuntimeError as e:
|
||
print(f"! {e}")
|
||
return
|
||
|
||
# 1.5 显式设定遥测流速率（不依赖默认配置；失败则沿用默认并告警）
|
||
for stream, rate in (("position", 50.0),
|
||
("position_velocity_ned", 30.0),
|
||
("attitude_euler", 50.0),
|
||
("in_air", 10.0)):
|
||
try:
|
||
await getattr(drone.telemetry, f"set_rate_{stream}")(rate)
|
||
except Exception as e:
|
||
print(f"! 设置 {stream} 速率失败（沿用默认值）: {e}")
|
||
print("-- 遥测速率已设定")
|
||
|
||
# 记录初始模式（可选，用于最后恢复）
|
||
initial_mode = await print_flight_mode(drone)
|
||
|
||
# 2. 解锁
|
||
print("-- 解锁")
|
||
try:
|
||
await drone.action.arm()
|
||
except ActionError as e:
|
||
print(f"解锁失败: {e}")
|
||
return
|
||
|
||
# 3. 起飞
|
||
print(f"-- 起飞到 {TAKEOFF_ALTITUDE} m")
|
||
await drone.action.set_takeoff_altitude(TAKEOFF_ALTITUDE)
|
||
await drone.action.takeoff()
|
||
|
||
# 等待起飞完成。注意：MAVSDK 遥测是带缓冲队列的推送流，循环体内
|
||
# 不能 sleep —— 否则消费速率（如 2 Hz）远低于推送速率（50 Hz），
|
||
# 队列积压导致读到的是几十秒前的旧高度（问题一的根因）。全速排水
|
||
# 使每条消息都是最新值，另加整体超时兜底。
|
||
print("-- 等待起飞完成...")
|
||
wait_start = time.monotonic()
|
||
next_log = wait_start + 2.0
|
||
reached = False
|
||
try:
|
||
async with asyncio.timeout(30.0):
|
||
async for position in drone.telemetry.position():
|
||
if position.relative_altitude_m >= TAKEOFF_ALTITUDE * 0.90:
|
||
wait_s = time.monotonic() - wait_start
|
||
print(f"-- 已到达起飞高度: {position.relative_altitude_m:.2f} m "
|
||
f"(等待 {wait_s:.1f} s)")
|
||
reached = True
|
||
break
|
||
now_t = time.monotonic()
|
||
if now_t >= next_log:
|
||
print(f" 起飞爬升中: {position.relative_altitude_m:.2f} m")
|
||
next_log = now_t + 2.0
|
||
except TimeoutError:
|
||
pass
|
||
if not reached:
|
||
print("! 30 s 内未达到起飞高度，放弃本次测试（执行降落兜底）")
|
||
try:
|
||
await drone.action.land()
|
||
except ActionError as e:
|
||
print(f"! 降落指令失败: {e}")
|
||
return
|
||
|
||
await asyncio.sleep(1)
|
||
|
||
# 4. 记录悬停锚点与锚点航向（虚拟目标按机头朝向放置）
|
||
try:
|
||
async with asyncio.timeout(10.0):
|
||
async for pos_vel in drone.telemetry.position_velocity_ned():
|
||
anchor_ned = (pos_vel.position.north_m,
|
||
pos_vel.position.east_m,
|
||
pos_vel.position.down_m)
|
||
break
|
||
anchor_yaw_deg = 0.0
|
||
async with asyncio.timeout(10.0):
|
||
async for euler in drone.telemetry.attitude_euler():
|
||
anchor_yaw_deg = euler.yaw_deg
|
||
break
|
||
except TimeoutError:
|
||
print("! 10 s 内未取得悬停锚点/航向，放弃本次测试（执行降落兜底）")
|
||
try:
|
||
await drone.action.land()
|
||
except ActionError as e:
|
||
print(f"! 降落指令失败: {e}")
|
||
return
|
||
print(f" 悬停锚点 NED: N={anchor_ned[0]:.2f}, "
|
||
f"E={anchor_ned[1]:.2f}, D={anchor_ned[2]:.2f}，"
|
||
f"机头 {anchor_yaw_deg:.1f}°")
|
||
|
||
# 5. 视觉跟踪闭环（offboard 速度设定点，20 Hz）
|
||
await run_tracking(drone, anchor_ned, anchor_yaw_deg)
|
||
|
||
# 6. 退出 offboard 后回到 Hold，稍候确认模式
|
||
await asyncio.sleep(1)
|
||
await print_flight_mode(drone)
|
||
|
||
# 7. 返航 (RTL)
|
||
print("-- 执行 RTL")
|
||
await drone.action.return_to_launch()
|
||
|
||
# 8. 等待落地（同样全速排水，避免 in_air 队列积压读到旧状态）
|
||
print("-- 等待落地...")
|
||
try:
|
||
async with asyncio.timeout(240.0):
|
||
async for is_in_air in drone.telemetry.in_air():
|
||
if not is_in_air:
|
||
print("-- 已落地")
|
||
break
|
||
except TimeoutError:
|
||
print("! 240 s 内未检测到落地")
|
||
|
||
await asyncio.sleep(2)
|
||
|
||
# 9. 上锁
|
||
print("-- 上锁")
|
||
try:
|
||
await drone.action.disarm()
|
||
print("-- 上锁成功")
|
||
except ActionError as e:
|
||
print(f"上锁失败（可能已自动上锁）: {e}")
|
||
|
||
print(f"-- 初始模式是: {initial_mode}")
|
||
print("\n===== 测试流程全部完成 =====")
|
||
|
||
if __name__ == "__main__":
|
||
logging.basicConfig(level=logging.INFO,
|
||
format="%(asctime)s.%(msecs)03d %(levelname)s %(message)s",
|
||
datefmt="%H:%M:%S")
|
||
asyncio.run(run())
|