keita
df91b1110e
归档 legacy/：stereo_yolo/stereo_ranger/target_tracker 三包 + 175 条测试 + demo/配置/权重/三份规格；运行链路不再依赖，默认 pytest 446 条，归档测试按需运行。 目标有效性第二判据改为 LOS 运动学一致性（pod_bridge/los_consistency.py），删除 require_vision；pod_locked() 叠加该判据，状态机随之后退重搜。 自定义返航（task_runner/return_home.py）：保持转场高度回起飞点 → 恢复初始航向 → 垂直降落，不再爬 RTL_RETURN_ALT；未起飞/无起飞点/超时/异常仍退回 PX4 原生 RTL。 任务链安全兜底：主循环异常→紧急返航、stop 幂等、landing_stop 保留遥测、in_air 流复用+超时、解锁/起飞与吊舱识别仅执行一次、rtl_timeout 上限。 入口共享件拆到 utils/sitl_support.py；pod_sitl_test.py→l1_sitl_test.py；删除旧架构 test.py；相机脚本移入 tools/。 新增 .gitignore 并剔除历史误跟踪的 59 个 .pyc；requirements 按实际依赖重写；文档全面对齐，新增 legacy/README.md 与 tools/README.md。
326 lines
12 KiB
Python
326 lines
12 KiB
Python
#!/usr/bin/env python3
|
||
"""l1_sitl_test.py —— L1 航向伺服 SITL 全流程集成测试。
|
||
|
||
连接 PX4 SITL，流程：
|
||
解锁 → 起飞 → 记录悬停锚点/航向 → 进 offboard
|
||
→ L1 航向伺服闭环 20 Hz（VirtualPod 按虚拟目标几何合成表2-2 遥测
|
||
→ PodBridge → TrackingControlLaw 角度直入 → offboard 速度设定点）
|
||
→ 目标消失 → 判丢零速刹车 → 退出 offboard → RTL → 落地 → 上锁
|
||
|
||
只覆盖 L1 控制律这一段；任务级全流程见根目录 `task_runner_sitl.py`。
|
||
|
||
要点：
|
||
* 走 L1 角度直入（吊舱脱靶量 off_target），无像素/检测框/追踪器；
|
||
* 无距离（distance_valid=False）→ 距离通道按 no_distance_action 降级
|
||
（slow_approach，0.2 m/s 极低限速）；
|
||
* 无电机 → 框架角恒 0；无激光 → laser=None；
|
||
* 符号使用部署配置（S1 标定：beta_sign=+1 / alpha_sign=-1），
|
||
本脚本即对该标定的端到端复核。
|
||
|
||
共享件（时钟/遥测订阅/连接准备）在 `utils/sitl_support.py`。
|
||
|
||
运行前：启动 PX4 SITL（默认本机 14540）。用法：
|
||
python l1_sitl_test.py
|
||
python l1_sitl_test.py --conn udpin://0.0.0.0:14540 --config utils/config/pod_bridge.json
|
||
"""
|
||
|
||
import argparse
|
||
import asyncio
|
||
import csv
|
||
import logging
|
||
import math
|
||
import sys
|
||
import time
|
||
from pathlib import Path
|
||
|
||
_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 pod_bridge.bridge import PodBridge
|
||
from pod_bridge.config import load_from_json
|
||
from pod_bridge.px4_gateway import Px4Gateway
|
||
from pod_bridge.virtual_pod import (CarrierView, VirtualPod, VirtualTarget,
|
||
VirtualTargetConfig)
|
||
|
||
logger = logging.getLogger("l1_sitl")
|
||
|
||
# ==================== 配置 ====================
|
||
CONNECTION = "udpin://0.0.0.0:14540"
|
||
CONFIG_PATH = _UTILS_DIR / "config" / "pod_bridge.json"
|
||
TAKEOFF_ALTITUDE = 5.0 # 起飞高度 (m)
|
||
|
||
|
||
# 虚拟目标：相对悬停锚点、按锚点机头朝向放置（前/右/下），随后水平圆弧运动。
|
||
TARGET_AHEAD_M = 30.0
|
||
TARGET_RIGHT_M = 15.0
|
||
TARGET_DOWN_M = 0.0 # 正 = 目标更低（地面目标场景，垂直通道会下降）
|
||
TARGET_ARC_RADIUS_M = 12.0
|
||
TARGET_ARC_SPEED_MPS = 1.0
|
||
TARGET_ARC_CCW = True
|
||
|
||
TRACK_NORMAL_SECS = 20.0 # 目标保持可见时长（之后模拟丢失）
|
||
TRACK_BRAKE_SECS = 5.0 # 判丢后零速刹车时长
|
||
TRACK_TIMEOUT_SECS = 120.0 # 跟踪段整体安全超时
|
||
STATUS_EVERY_TICKS = 10 # 20 Hz 下每 0.5 s 打印一行状态
|
||
CSV_PATH = Path(__file__).resolve().parent / "l1_sitl_trajectory.csv"
|
||
|
||
|
||
|
||
# 共享件（时钟/遥测订阅/连接准备）：utils/sitl_support.py
|
||
from sitl_support import (TelemetryHub, now_ms, print_flight_mode,
|
||
set_telemetry_rates, wait_until_ready)
|
||
|
||
|
||
async def takeoff(drone) -> bool:
|
||
"""解锁 + 起飞到 TAKEOFF_ALTITUDE。失败返回 False（调用方执行降落兜底）。"""
|
||
print("-- 解锁")
|
||
try:
|
||
await drone.action.arm()
|
||
except ActionError as e:
|
||
print(f"解锁失败: {e}")
|
||
return False
|
||
|
||
print(f"-- 起飞到 {TAKEOFF_ALTITUDE} m")
|
||
await drone.action.set_takeoff_altitude(TAKEOFF_ALTITUDE)
|
||
await drone.action.takeoff()
|
||
|
||
# 全速排水等待爬升（test.py 问题一的经验：循环内不 sleep，避免队列积压）
|
||
print("-- 等待起飞完成...")
|
||
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:
|
||
print(f"-- 已到达起飞高度: {position.relative_altitude_m:.2f} m")
|
||
reached = True
|
||
break
|
||
except TimeoutError:
|
||
pass
|
||
if not reached:
|
||
print("! 30 s 内未达到起飞高度")
|
||
return reached
|
||
|
||
|
||
async def land_fallback(drone):
|
||
try:
|
||
await drone.action.land()
|
||
except ActionError as e:
|
||
print(f"! 降落指令失败: {e}")
|
||
|
||
|
||
async def read_anchor(drone):
|
||
"""读取悬停锚点 NED 与机头航向（虚拟目标按机头朝向放置）。"""
|
||
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
|
||
async with asyncio.timeout(10.0):
|
||
async for euler in drone.telemetry.attitude_euler():
|
||
anchor_yaw_deg = euler.yaw_deg
|
||
break
|
||
except TimeoutError:
|
||
raise RuntimeError("10 s 内未取得悬停锚点/航向") from None
|
||
print(f" 悬停锚点 NED: N={anchor_ned[0]:.2f}, E={anchor_ned[1]:.2f}, "
|
||
f"D={anchor_ned[2]:.2f}，机头 {anchor_yaw_deg:.1f}°")
|
||
return anchor_ned, anchor_yaw_deg
|
||
|
||
|
||
async def run_tracking(bridge, hub, target, started_ms: int) -> bool:
|
||
"""L1 航向伺服闭环段：按生效律频率循环 bridge.step()，返回是否正常走完场景。"""
|
||
period_s = 1.0 / bridge._active_frequency_hz()
|
||
target_gone_ms = started_ms + TRACK_NORMAL_SECS * 1000.0
|
||
lost_at_ms = None
|
||
next_deadline = time.monotonic() + period_s
|
||
tick = 0
|
||
rows: list = []
|
||
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("-- 模拟目标消失，吊舱转搜索态 → 应判丢刹车")
|
||
|
||
try:
|
||
out = await bridge.step(now)
|
||
except Exception as exc:
|
||
# 对齐 bridge.run() 的护栏：异常不终止，补发刹车保 offboard 心跳
|
||
logger.warning("[SITL] 控制周期异常: %s", exc)
|
||
try:
|
||
await bridge._px4.send_brake_hover(now)
|
||
except Exception:
|
||
pass
|
||
await asyncio.sleep(period_s)
|
||
continue
|
||
|
||
if tick % STATUS_EVERY_TICKS == 0:
|
||
t_s = (now - started_ms) / 1000.0
|
||
tel = bridge.latest_telemetry
|
||
off = f"off=({tel[0].off_target_x_deg:+.1f},{tel[0].off_target_y_deg:+.1f})°" \
|
||
if tel else "off=(无遥测)"
|
||
print(f"[跟踪 t={t_s:5.1f}s] {off} | [{out.mode.name}] "
|
||
f"yaw_rate={out.yaw_rate_dps:+6.2f}°/s "
|
||
f"vx_body={out.vx_body_mps:+.2f} vz={out.vz_mps:+.2f}")
|
||
rows.append((t_s, hub.north_m, hub.east_m, hub.down_m,
|
||
*target.pos_ned, out.yaw_rate_dps,
|
||
out.vx_body_mps, out.vz_mps, out.mode.name))
|
||
tick += 1
|
||
|
||
if out.mode.name == "BRAKE_HOVER":
|
||
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:
|
||
_write_csv(rows)
|
||
return True
|
||
|
||
|
||
def _write_csv(rows) -> None:
|
||
if not rows:
|
||
return
|
||
try:
|
||
with open(CSV_PATH, "w", newline="", encoding="utf-8") as fp:
|
||
w = csv.writer(fp)
|
||
w.writerow(["t_s", "drone_n", "drone_e", "drone_d",
|
||
"tgt_n", "tgt_e", "tgt_d",
|
||
"yaw_rate_dps", "vx_body", "vz", "mode"])
|
||
w.writerows(rows)
|
||
print(f"-- 轨迹 CSV 已保存: {CSV_PATH}")
|
||
except OSError as e:
|
||
print(f"! CSV 写入失败: {e}")
|
||
|
||
|
||
async def run(args) -> int:
|
||
cfg = load_from_json(args.config)
|
||
if not cfg.heading_servo.enabled:
|
||
print("! 配置 heading_servo.enabled=false：本脚本跑的是 L1 路径，"
|
||
"请用 pod_bridge.json（enabled=true）")
|
||
return 2
|
||
|
||
drone = System()
|
||
await drone.connect(system_address=args.conn)
|
||
|
||
try:
|
||
await wait_until_ready(drone)
|
||
except RuntimeError as e:
|
||
print(f"! {e}")
|
||
return 1
|
||
|
||
await set_telemetry_rates(drone)
|
||
initial_mode = await print_flight_mode(drone)
|
||
|
||
if not await takeoff(drone):
|
||
await land_fallback(drone)
|
||
return 1
|
||
|
||
await asyncio.sleep(1)
|
||
try:
|
||
anchor_ned, anchor_yaw_deg = await read_anchor(drone)
|
||
except RuntimeError as e:
|
||
print(f"! {e}")
|
||
await land_fallback(drone)
|
||
return 1
|
||
|
||
# ---- L1 闭环组件装配：真 SITL 飞控 + 模拟吊舱，全部复用生产代码路径 ----
|
||
hub = TelemetryHub()
|
||
hub.start(drone)
|
||
target = VirtualTarget(
|
||
VirtualTargetConfig(ahead_m=TARGET_AHEAD_M, right_m=TARGET_RIGHT_M,
|
||
down_m=TARGET_DOWN_M,
|
||
arc_radius_m=TARGET_ARC_RADIUS_M,
|
||
arc_speed_mps=TARGET_ARC_SPEED_MPS,
|
||
arc_ccw=TARGET_ARC_CCW),
|
||
anchor_ned, math.radians(anchor_yaw_deg))
|
||
pod = VirtualPod(target, hub.view,
|
||
fov_h_deg=cfg.tracking.camera.fov_h_deg,
|
||
fov_v_deg=cfg.tracking.camera.fov_v_deg)
|
||
px4 = Px4Gateway(drone)
|
||
bridge = PodBridge(cfg, px4, pod, now_ms_fn=now_ms)
|
||
|
||
print(f"-- 进入 L1 航向伺服：虚拟目标按机头朝向放置于 "
|
||
f"前 {TARGET_AHEAD_M:.0f} m / 右 {TARGET_RIGHT_M:.0f} m / "
|
||
f"下 {TARGET_DOWN_M:.0f} m（世界 NED: N={target.pos_ned[0]:.1f}, "
|
||
f"E={target.pos_ned[1]:.1f}, D={target.pos_ned[2]:.1f}）")
|
||
print(f" 场景: 正常跟踪 {TRACK_NORMAL_SECS:.0f}s → 目标消失 → "
|
||
f"判丢刹车 {TRACK_BRAKE_SECS:.0f}s")
|
||
print(f" 符号(部署配置): beta_sign={cfg.heading_servo.beta_sign:+d} "
|
||
f"alpha_sign={cfg.heading_servo.alpha_sign:+d} | 目标偏右→yaw_rate 正，"
|
||
f"目标偏下→vz 正")
|
||
|
||
if not await bridge.start():
|
||
print("! offboard 启动失败，放弃跟踪段")
|
||
await bridge.stop()
|
||
await hub.stop()
|
||
await land_fallback(drone)
|
||
return 1
|
||
|
||
await run_tracking(bridge, hub, target, now_ms())
|
||
await bridge.stop()
|
||
await hub.stop()
|
||
|
||
# ---- 退出 offboard 后回到 Hold，稍候确认模式 ----
|
||
await asyncio.sleep(1)
|
||
await print_flight_mode(drone)
|
||
|
||
# ---- RTL → 等待落地 → 上锁 ----
|
||
print("-- 执行 RTL")
|
||
await drone.action.return_to_launch()
|
||
|
||
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)
|
||
print("-- 上锁")
|
||
try:
|
||
await drone.action.disarm()
|
||
print("-- 上锁成功")
|
||
except ActionError as e:
|
||
print(f"上锁失败（可能已自动上锁）: {e}")
|
||
|
||
print(f"-- 初始模式是: {initial_mode}")
|
||
print("\n===== L1 全流程测试完成 =====")
|
||
return 0
|
||
|
||
|
||
def main() -> int:
|
||
ap = argparse.ArgumentParser(description="L1 航向伺服 SITL 全流程集成测试")
|
||
ap.add_argument("--conn", default=CONNECTION, help="MAVSDK 连接串（默认 SITL 14540）")
|
||
ap.add_argument("--config", default=str(CONFIG_PATH), help="桥接配置 JSON 路径")
|
||
args = ap.parse_args()
|
||
|
||
logging.basicConfig(level=logging.INFO,
|
||
format="%(asctime)s.%(msecs)03d %(levelname)s %(message)s",
|
||
datefmt="%H:%M:%S")
|
||
return asyncio.run(run(args))
|
||
|
||
|
||
if __name__ == "__main__":
|
||
sys.exit(main())
|