first commit
This commit is contained in:
@@ -0,0 +1,207 @@
|
||||
"""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()
|
||||
Reference in New Issue
Block a user