208 lines
8.4 KiB
Python
208 lines
8.4 KiB
Python
"""demo_track_control.py —— 视觉跟踪控制链路集成演示（胶水层）。
|
||
|
||
对应 C++ 参考实现 reference-c++/visual_track_control/examples/track_control_demo.cpp。
|
||
|
||
串联三个组件（target_tracker 与 visual_track_control 互不 import，
|
||
本脚本是唯一的转换点）：
|
||
模拟检测序列 → TargetTracker（8 维恒速卡尔曼多目标追踪）
|
||
→ TrackResult 转 TrackState（解耦边界）
|
||
→ TrackingControlLaw（三通道控制律 + 限幅 + 状态机）
|
||
→ CommandSender（mock 记录 / MAVSDK offboard 实发）
|
||
|
||
场景时间线（20 Hz，--speedup 可加速）：
|
||
0~ 6s 正常跟踪：目标匀速横移 + 雷达距离 30→10 m 前向逼近
|
||
6~10s 目标消失：追踪器 Coast 预测（60 帧 = 3 s 后轨道判丢，
|
||
控制律自动切换零速刹车）
|
||
10~13s 持续判丢：维持零速刹车
|
||
|
||
用法：
|
||
python utils/demo_track_control.py # mock 传输
|
||
python utils/demo_track_control.py --transport mavsdk # MAVSDK（需飞控/SITL）
|
||
python utils/demo_track_control.py --speedup 4 # 4 倍速
|
||
"""
|
||
|
||
import argparse
|
||
import asyncio
|
||
import logging
|
||
from pathlib import Path
|
||
|
||
from target_tracker.target_tracker import Detection, TargetTracker, TrackerConfig
|
||
from visual_track_control.config import load_from_json
|
||
from visual_track_control.mavsdk_gateway import MavsdkCommandGateway
|
||
from visual_track_control.tracking_control_law import TrackingControlLaw
|
||
from visual_track_control.types import (ControlMode, ControlSnapshot,
|
||
ControlOutput, TrackState)
|
||
|
||
logger = logging.getLogger("demo")
|
||
|
||
_DEMO_DIR = Path(__file__).resolve().parent
|
||
_DEFAULT_CONFIG = _DEMO_DIR / "config" / "track_control.json"
|
||
|
||
# 模拟场景参数
|
||
_NORMAL_SECS = 6.0 # 正常跟踪阶段
|
||
_COAST_SECS = 4.0 # 目标消失 → Coast（含轨道判亡）
|
||
_LOST_SECS = 3.0 # 判丢后持续刹车阶段
|
||
_DEMO_MAX_LOST_FRAMES = 60 # 追踪器 Coast 窗口（20 Hz 下 = 3 s）
|
||
_STATUS_EVERY_TICKS = 10 # 每 0.5 s 打印一行状态
|
||
|
||
|
||
def track_result_to_state(result) -> TrackState:
|
||
"""两包之间的唯一转换点：TrackResult（box 为对象）→ TrackState（plain tuple）。"""
|
||
box = result.box
|
||
raw = result.raw
|
||
return TrackState(
|
||
tracked=result.tracked,
|
||
primary_id=result.primary_id,
|
||
center=result.center,
|
||
box=(box.x1, box.y1, box.x2, box.y2) if box is not None else None,
|
||
confidence=result.confidence,
|
||
is_predicted=result.is_predicted,
|
||
lost_frames=result.lost_frames,
|
||
n_active=result.n_active,
|
||
raw=(raw.x1, raw.y1, raw.x2, raw.y2) if raw is not None else None,
|
||
)
|
||
|
||
|
||
def simulate_detections(tick: int, normal_ticks: int, period_s: float):
|
||
"""场景检测源：正常阶段目标匀速横移，之后消失。"""
|
||
if tick >= normal_ticks:
|
||
return []
|
||
cx = 960.0 + 3.0 * tick # 3 px/帧 匀速右移
|
||
cy = 540.0
|
||
w, h = 120.0, 160.0
|
||
return [Detection(cx - w / 2.0, cy - h / 2.0, cx + w / 2.0, cy + h / 2.0, 0.9)]
|
||
|
||
|
||
def simulate_radar(tick: int, normal_ticks: int, period_s: float):
|
||
"""场景雷达源：正常阶段关联成立、距离 30→10 m；之后未关联。"""
|
||
if tick >= normal_ticks:
|
||
return 0.0, False
|
||
distance = max(10.0, 30.0 - tick * period_s * 3.0)
|
||
return distance, True
|
||
|
||
|
||
class MockCommandSender:
|
||
"""CommandSender 的 mock 实现：记录最近一条指令描述（无实机验证能力）。"""
|
||
|
||
def __init__(self) -> None:
|
||
self.last_desc = "-"
|
||
|
||
async def start(self) -> bool:
|
||
logger.info("[MockTransport] offboard 启动（模拟）")
|
||
return True
|
||
|
||
async def send_velocity_heading(self, output: ControlOutput,
|
||
time_boot_ms=None) -> bool:
|
||
self.last_desc = (f"vx={output.vx_mps:+.2f} vy={output.vy_mps:+.2f} "
|
||
f"vz={output.vz_mps:+.2f} "
|
||
f"yaw_rate={output.yaw_rate_dps:+.2f}°/s "
|
||
f"yaw={output.yaw_deg:+.2f}°")
|
||
return True
|
||
|
||
async def send_brake_hover(self, time_boot_ms=None) -> bool:
|
||
self.last_desc = "零速刹车（全零机体速度设定点）"
|
||
return True
|
||
|
||
async def stop(self) -> None:
|
||
logger.info("[MockTransport] offboard 停止（模拟）")
|
||
|
||
|
||
async def make_sender(args) -> object:
|
||
if args.transport == "mock":
|
||
return MockCommandSender()
|
||
from mavsdk import System
|
||
drone = System()
|
||
logger.info("连接飞控 %s ...", args.conn)
|
||
await drone.connect(system_address=args.conn)
|
||
async for state in drone.core.connection_state():
|
||
if state.is_connected:
|
||
logger.info("飞控已连接")
|
||
break
|
||
return MavsdkCommandGateway(drone)
|
||
|
||
|
||
async def dispatch(sender, out: ControlOutput, now_ms: int):
|
||
"""按模式分发指令；DEGRADED_HOLD 不发送（禁止正常控制）。"""
|
||
if out.mode is ControlMode.VELOCITY_HEADING and out.valid:
|
||
return await sender.send_velocity_heading(out, now_ms)
|
||
if out.mode is ControlMode.BRAKE_HOVER and out.valid:
|
||
return await sender.send_brake_hover(now_ms)
|
||
return None
|
||
|
||
|
||
async def run_demo(args) -> None:
|
||
cfg = load_from_json(args.config)
|
||
period_s = 1.0 / cfg.control.frequency_hz
|
||
period_ms = int(round(period_s * 1000.0))
|
||
|
||
tracker = TargetTracker(TrackerConfig(max_lost_frames=_DEMO_MAX_LOST_FRAMES))
|
||
law = TrackingControlLaw(cfg)
|
||
sender = await make_sender(args)
|
||
|
||
normal_ticks = int(round(_NORMAL_SECS / period_s))
|
||
coast_ticks = int(round(_COAST_SECS / period_s))
|
||
lost_ticks = int(round(_LOST_SECS / period_s))
|
||
total_ticks = normal_ticks + coast_ticks + lost_ticks
|
||
|
||
if not await sender.start():
|
||
logger.error("offboard 启动失败，退出")
|
||
return
|
||
|
||
logger.info("开始 %d 拍（%.1f s，%.1f Hz）：正常 %.1fs → Coast %.1fs → 判丢 %.1fs",
|
||
total_ticks, total_ticks * period_s, cfg.control.frequency_hz,
|
||
_NORMAL_SECS, _COAST_SECS, _LOST_SECS)
|
||
try:
|
||
for tick in range(total_ticks):
|
||
now_ms = 1000 + tick * period_ms
|
||
detections = simulate_detections(tick, normal_ticks, period_s)
|
||
radar_m, associated = simulate_radar(tick, normal_ticks, period_s)
|
||
|
||
result = tracker.update(detections)
|
||
snap = ControlSnapshot(
|
||
track=track_result_to_state(result),
|
||
track_time_ms=now_ms, # 每拍取样即打戳（视觉始终新鲜）
|
||
radar_distance_m=radar_m,
|
||
associated_to_target=associated,
|
||
radar_time_ms=now_ms if associated else 0,
|
||
attitude_present=True,
|
||
attitude_time_ms=now_ms, # 模拟姿态源持续可用
|
||
yaw_rad=0.0, # 机头朝北（NED 旋转恒等）
|
||
)
|
||
out = law.update(snap, now_ms)
|
||
await dispatch(sender, out, now_ms)
|
||
|
||
if tick % _STATUS_EVERY_TICKS == 0:
|
||
logger.info(
|
||
"t=%5.2fs 检测=%d 追踪[trk=%d pred=%d lost=%d] | "
|
||
"[%s] vx=%+.2f vy=%+.2f vz=%+.2f yaw_rate=%+.2f coast=%d | 发送: %s",
|
||
(tick * period_s), len(detections), result.tracked,
|
||
result.is_predicted, result.lost_frames, out.mode.name,
|
||
out.vx_mps, out.vy_mps, out.vz_mps, out.yaw_rate_dps,
|
||
out.coast_active, sender.last_desc)
|
||
await asyncio.sleep(period_s / args.speedup)
|
||
finally:
|
||
await sender.stop()
|
||
logger.info("演示结束")
|
||
|
||
|
||
def main() -> None:
|
||
parser = argparse.ArgumentParser(description="视觉跟踪控制链路集成演示")
|
||
parser.add_argument("--transport", choices=["mock", "mavsdk"], default="mock",
|
||
help="指令发送方式：mock=本地记录，mavsdk=offboard 实发")
|
||
parser.add_argument("--conn", default="udp://:14540",
|
||
help="MAVSDK 连接串（仅 --transport mavsdk 生效）")
|
||
parser.add_argument("--speedup", type=float, default=1.0,
|
||
help="时间倍速（>1 加速演示）")
|
||
parser.add_argument("--config", default=str(_DEFAULT_CONFIG),
|
||
help="控制律配置文件路径")
|
||
args = parser.parse_args()
|
||
|
||
logging.basicConfig(level=logging.INFO,
|
||
format="%(asctime)s.%(msecs)03d %(levelname)s %(message)s",
|
||
datefmt="%H:%M:%S")
|
||
asyncio.run(run_demo(args))
|
||
|
||
|
||
if __name__ == "__main__":
|
||
main()
|