"""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_distance(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) distance_m, distance_valid = simulate_distance(tick, normal_ticks, period_s) result = tracker.update(detections) snap = ControlSnapshot( track=track_result_to_state(result), track_time_ms=now_ms, # 每拍取样即打戳（视觉始终新鲜） target_distance_m=distance_m, distance_valid=distance_valid, distance_time_ms=now_ms, 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()