685 lines
29 KiB
Python
685 lines
29 KiB
Python
"""
|
||
run_mission.py — 基于 ZMQ Router 协议的主任务入口
|
||
|
||
【架构概览】
|
||
本脚本是整个无人机任务系统的总装粘合剂。采用领域驱动模块化设计，
|
||
将各子系统按职责独立实例化，在主循环中进行控制权协调。
|
||
|
||
┌─ MissionOrchestrator ──────────┐
|
||
│ RouterProxy (shared) │ 单一共享 ZMQ 代理
|
||
└──────────┬─────────────────────┘
|
||
│ 注入
|
||
┌─────┴──────┐
|
||
▼ ▼
|
||
┌─ MissionManager ┌─ UAVControlLoop ──┐
|
||
│ 状态机 + 航点巡航 │ 20Hz VELOCITY │
|
||
│ POSITION SETPOINT │ 闭环追踪 │
|
||
│ (非追踪期间) │ (VISUAL_TRACKING) │
|
||
└───────────────────┴────────────────────┘
|
||
┌─ 感知模块 ─────────┐
|
||
│ VideoStreaming │ 摄像头/RTSP 采集
|
||
│ YOLO26UAVInfer │ 昇腾 NPU 推理
|
||
└───────────────────┘
|
||
|
||
【控制权协调规则】
|
||
同一时刻只有一个人持有「SETPOINT 发令权」：
|
||
- VISUAL_TRACKING 之外 → MissionManager 发 POSITION SETPOINT
|
||
- VISUAL_TRACKING 之中 → UAVControlLoop 线程发 VELOCITY SETPOINT
|
||
- 切换由主循环检测 MissionManager.state 变化时触发
|
||
|
||
【安全设计】
|
||
- try / except FailsafeTriggered / finally 全面资源释放
|
||
- MissionManager 和 UAVControlLoop 共享同一 RouterProxy，
|
||
通过控制权协调避免 REQ socket 并发冲突
|
||
- 控制线程启停由状态机状态变化驱动，逻辑可预测
|
||
|
||
【可扩展的图像输入接口】
|
||
如需替换视频源（如模拟器/录播文件），替换 VideoStreaming 实例
|
||
即可，保持 read_frame() → (ret, frame) 的协议不变。
|
||
"""
|
||
|
||
import json
|
||
import logging
|
||
import os
|
||
import sys
|
||
import threading
|
||
import time
|
||
import socket
|
||
import cv2
|
||
|
||
from drone_controller.mission_manager import (
|
||
MissionManager,
|
||
MissionState,
|
||
FailsafeTriggered,
|
||
)
|
||
# ---- ZMQ Router 代理与状态机 ----
|
||
from drone_controller.router_proxy import RouterProxy
|
||
# ---- 视觉闭环控制（复用 infer_camera_modular 现有实现） ----
|
||
# 注意：此处不修改 UAVControlLoop 任何内部逻辑。
|
||
# run_mission.py 在外部协调其控制线程的启停。
|
||
from infer_camera_modular import UAVControlLoop, VideoStreaming, YOLO26UAVInfer
|
||
# 注：coord.latlon_to_ned 不再需要 — 航点直接存储原始经纬度
|
||
# ---- 飞行日志 ----
|
||
from utils.flight_logger import FlightLogger
|
||
# ---- 光流模块 ----
|
||
from drone_controller.optical_flow_controller import OpticalFlowObstacleDetector
|
||
|
||
logger = logging.getLogger("RunMission")
|
||
|
||
|
||
class MissionOrchestrator:
|
||
"""
|
||
🎯 全任务编排器
|
||
|
||
职责：
|
||
1. 创建并初始化所有子系统
|
||
2. 在主循环中协调 MissionManager 状态机与 UAVControlLoop 的控制权
|
||
3. 处理 Failsafe 熔断、键盘中断等异常退出
|
||
4. 提供标准视频源接口（可替换）
|
||
"""
|
||
|
||
def __init__(self,waypoints,config_path="./config.json"):
|
||
self.logger = logging.getLogger("Orchestrator")
|
||
self.logger.info("正在加载配置: %s", config_path)
|
||
|
||
with open(config_path, "r", encoding="utf-8") as f:
|
||
cfg = json.load(f)
|
||
self.cfg = cfg
|
||
|
||
# ---- 感知子系统 ----
|
||
self.streamer = VideoStreaming(cfg["video"], cfg["network"])
|
||
self.detector = YOLO26UAVInfer(cfg["model"])
|
||
|
||
# ---- 航线定义（经纬度坐标系，Z 向上为负） ----
|
||
self.waypoints = waypoints
|
||
|
||
# ================================================================
|
||
# csv飞行日志参数
|
||
# ================================================================
|
||
log_cfg = cfg.get("log", {})
|
||
self.flight_logger = FlightLogger(log_cfg)
|
||
self._latest_detections = None
|
||
|
||
# ================================================================
|
||
# 🚁 MissionManager（状态机 + 航点巡航）
|
||
# ================================================================
|
||
# 与 UAVControlLoop 共享同一个 RouterProxy（见下方）。
|
||
fc = cfg["flight_control"]
|
||
self.proxy = RouterProxy(
|
||
req_endpoint=fc.get("req_endpoint", "tcp://127.0.0.1:5555"),
|
||
sub_endpoint=fc.get("sub_endpoint", "tcp://127.0.0.1:5556"),
|
||
)
|
||
|
||
self.proxy.set_waypoints_callback(self._on_qgc_waypoints)
|
||
|
||
missions = cfg["missions"]
|
||
landing_cfg = missions.get("landing", {})
|
||
self.mission = MissionManager(
|
||
proxy=self.proxy,
|
||
waypoints=self.waypoints,
|
||
target_altitude=missions.get("target_altitude", 3.0),
|
||
arrival_radius=missions.get("arrival_radius", 0.3),
|
||
hold_duration=missions.get("hold_duration", 5.0),
|
||
return_to_home=missions.get("return_to_home", True),
|
||
max_speed=missions.get("max_speed", 1.2),
|
||
pre_land_hover_duration=landing_cfg.get("pre_hover_duration", 3.0),
|
||
landing_descent_speed=landing_cfg.get("descent_speed", 0.3),
|
||
)
|
||
|
||
# ================================================================
|
||
# 💡 OpticalFlowObstacleDetector （光流计算）
|
||
# ================================================================
|
||
flow_cfg = cfg.get("optical_flow", {})
|
||
self.optical_flow = OpticalFlowObstacleDetector(
|
||
grid_step=flow_cfg.get("grid_step", 25),
|
||
reset_threshold=flow_cfg.get("reset_threshold", 30.0)
|
||
)
|
||
|
||
# ================================================================
|
||
# 🛸 UAVControlLoop（20Hz VELOCITY 闭环追踪）
|
||
# ================================================================
|
||
# 注入与 MissionManager 相同的 RouterProxy 实例，避免两个独立
|
||
# SUB 线程和 REQ socket 竞争同一 Router。
|
||
self.controller = UAVControlLoop(
|
||
cfg["flight_control"], cfg["pid_y"], cfg["pid_z"],
|
||
proxy=self.proxy,
|
||
)
|
||
self._control_active = False # 控制线程运行标志
|
||
|
||
# ---- 主循环参数 ----
|
||
self.running = False
|
||
self.loop_interval = 0.05 # 20Hz
|
||
# ---- TCP 命令通道（急停 / 后续扩展） ----
|
||
self.tcp_port = cfg.get("command_channel", {}).get("tcp_port", 9999)
|
||
self.tcp_server_socket = None
|
||
self.tcp_server_running = False
|
||
self.tcp_server_thread = None
|
||
self.tcp_clients = [] # 保存已连接的客户端 socket（用于广播）
|
||
|
||
self.logger.info("MissionOrchestrator 初始化完成")
|
||
|
||
# ================================================================
|
||
# 📥 QGC 航点回调（由 RouterProxy 的 SUB 线程触发）
|
||
# ================================================================
|
||
|
||
def _on_qgc_waypoints(self, data: dict):
|
||
"""
|
||
收到 QGC 下发的航点列表 → 直接存储原始经纬度 → 注入 MissionManager
|
||
|
||
【协议变更 v2】
|
||
航点格式已从 NED (x, y, z) 切换为经纬度 (lat, lon, alt)。
|
||
send_setpoint 的 x=纬度, y=经度, z=相对高度（正值向上）。
|
||
因此不再调用 latlon_to_ned 转换，直接存储原始数据。
|
||
"""
|
||
try:
|
||
raw_wps = data.get("waypoints", [])
|
||
if not raw_wps:
|
||
self.logger.warning("QGC 航点列表为空，请检查是否提交或路由问题")
|
||
return
|
||
|
||
self.logger.info("📥 收到 %d 个航点，直接存储经纬度", len(raw_wps))
|
||
|
||
# ---- ① 获取 STATE 用于后续校验（不再需要参考原点） ----
|
||
state = self.proxy.get_latest_state()
|
||
if state is None:
|
||
self.logger.warning("无法获取 STATE，航点仍将加载但 HOME 校验跳过")
|
||
return
|
||
|
||
# ---- ② 逐点存储（原始经纬度，不做 NED 转换） ----
|
||
converted = []
|
||
for idx, wp in enumerate(raw_wps):
|
||
lat = wp.get("lat", 0.0)
|
||
lon = wp.get("lon", 0.0)
|
||
alt = wp.get("z", 0.0) # QGC 中 z 为相对高度（正值向上）
|
||
cmd = wp.get("command", 16) # 16=WAYPOINT, 22=TAKEOFF, 21=LAND
|
||
yaw = wp.get("param4", 0.0) # param4 通常存偏航角
|
||
hold_time = wp.get("param1", 8.0) # 悬停时间（如有）
|
||
|
||
# 根据 MAV_CMD 类型构建内部航点
|
||
if cmd == 22: # TAKEOFF
|
||
converted.append({
|
||
"lat": lat,
|
||
"lon": lon,
|
||
"alt": alt,
|
||
"yaw": yaw,
|
||
"command": "TAKEOFF",
|
||
"hold_duration": hold_time,
|
||
})
|
||
self.logger.info("航点 %d [TAKEOFF]: lat=%.6f lon=%.6f alt=%.1fm",
|
||
idx, lat, lon, alt)
|
||
|
||
elif cmd == 21: # LAND
|
||
converted.append({
|
||
"lat": lat,
|
||
"lon": lon,
|
||
"alt": alt,
|
||
"yaw": yaw,
|
||
"command": "LAND",
|
||
})
|
||
self.logger.info("航点 %d [LAND]: lat=%.6f lon=%.6f", idx, lat, lon)
|
||
|
||
else: # 普通航点 (command=16 或未识别)
|
||
converted.append({
|
||
"lat": lat,
|
||
"lon": lon,
|
||
"alt": alt,
|
||
"yaw": yaw,
|
||
"command": "WAYPOINT",
|
||
"hold_duration": hold_time,
|
||
})
|
||
self.logger.debug("航点 %d [WAYPOINT]: lat=%.6f lon=%.6f alt=%.1f",
|
||
idx, lat, lon, alt)
|
||
|
||
self.mission.set_waypoints(converted)
|
||
|
||
# 如果当前处于等待航点状态，状态机会在下一帧自动响应
|
||
self.logger.info("✅ 航点加载完成，共 %d 个航点（原始经纬度格式）", len(converted))
|
||
|
||
except Exception as e:
|
||
self.logger.error("航点处理异常: %s", e)
|
||
|
||
# ================================================================
|
||
# 🖥️ TCP Server
|
||
# ================================================================
|
||
# run_mission.py — MissionOrchestrator 类中新增
|
||
|
||
def _start_tcp_command_server(self):
|
||
"""启动 TCP 命令服务器（独立线程）"""
|
||
if self.tcp_server_thread and self.tcp_server_thread.is_alive():
|
||
return
|
||
|
||
self.tcp_server_running = True
|
||
self.tcp_server_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
|
||
self.tcp_server_socket.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
|
||
self.tcp_server_socket.bind(("0.0.0.0", self.tcp_port))
|
||
self.tcp_server_socket.listen(5)
|
||
self.tcp_server_socket.settimeout(1.0) # 用于检查 self.tcp_server_running
|
||
|
||
self.tcp_server_thread = threading.Thread(
|
||
target=self._tcp_command_server_worker,
|
||
daemon=True,
|
||
name="tcp-command-server"
|
||
)
|
||
self.tcp_server_thread.start()
|
||
self.logger.info(f"🔌 TCP 命令通道已启动: port={self.tcp_port} (指令: STOP)")
|
||
|
||
def _tcp_command_server_worker(self):
|
||
"""TCP 服务器主循环：接受客户端，分发处理"""
|
||
while self.tcp_server_running:
|
||
try:
|
||
client_sock, addr = self.tcp_server_socket.accept()
|
||
self.logger.info(f"📡 地面站已连接: {addr[0]}:{addr[1]}")
|
||
# 为每个客户端创建独立处理线程（支持多地面站同时连接）
|
||
client_thread = threading.Thread(
|
||
target=self._handle_tcp_client,
|
||
args=(client_sock, addr),
|
||
daemon=True
|
||
)
|
||
client_thread.start()
|
||
except socket.timeout:
|
||
continue
|
||
except Exception as e:
|
||
if self.tcp_server_running:
|
||
self.logger.error(f"TCP Server 异常: {e}")
|
||
break
|
||
|
||
# 清理
|
||
if self.tcp_server_socket:
|
||
try:
|
||
self.tcp_server_socket.close()
|
||
except Exception:
|
||
pass
|
||
self.tcp_server_socket = None
|
||
self.logger.info("🔌 TCP 命令通道已关闭")
|
||
|
||
def _handle_tcp_client(self, client_sock, addr):
|
||
"""处理单个 TCP 客户端连接（长连接）"""
|
||
client_sock.settimeout(1.0) # 读超时，便于检测连接断开
|
||
try:
|
||
while self.tcp_server_running:
|
||
try:
|
||
data = client_sock.recv(1024).decode().strip()
|
||
if not data:
|
||
# 客户端主动断开
|
||
self.logger.info(f"📡 地面站断开: {addr[0]}:{addr[1]}")
|
||
break
|
||
|
||
msg = data.upper()
|
||
self.logger.info(f"📨 收到指令: {msg} (来自 {addr[0]}:{addr[1]})")
|
||
|
||
if msg == "STOP":
|
||
self.logger.warning(f"🚨 触发紧急降落 (指令来自 {addr[0]}:{addr[1]})")
|
||
self.mission.emergency_stop()
|
||
self.running = False
|
||
client_sock.send(b"ACK: EMERGENCY_STOP_TRIGGERED\n")
|
||
# 触发后不立即断开，让客户端收到 ACK
|
||
# 但主循环即将退出，服务器也会关闭
|
||
|
||
elif msg == "PING":
|
||
# 心跳 / 保活响应
|
||
client_sock.send(b"PONG\n")
|
||
|
||
elif msg == "STATUS":
|
||
# 返回当前状态（便于调试）
|
||
status = f"STATE:{self.mission.state.name}, WP:{self.mission.current_wp_index}/{len(self.mission.waypoints)}\n"
|
||
client_sock.send(status.encode())
|
||
|
||
else:
|
||
client_sock.send(f"UNKNOWN_CMD: {msg}\n".encode())
|
||
|
||
except socket.timeout:
|
||
# 读超时，继续循环检查 self.tcp_server_running
|
||
continue
|
||
except BrokenPipeError:
|
||
self.logger.warning(f"📡 地面站连接异常断开: {addr[0]}:{addr[1]}")
|
||
break
|
||
except Exception as e:
|
||
self.logger.error(f"TCP 客户端处理异常: {e}")
|
||
break
|
||
finally:
|
||
try:
|
||
client_sock.close()
|
||
except Exception:
|
||
pass
|
||
|
||
# ================================================================
|
||
# 🔌 启动
|
||
# ================================================================
|
||
|
||
def start(self):
|
||
"""启动所有子系统"""
|
||
self.logger.info("正在启动所有子系统...")
|
||
|
||
# 1. 启动共享 ZMQ 代理（MissionManager 与 UAVControlLoop 共用）
|
||
self.proxy.start()
|
||
self.logger.info("✔ 共享 RouterProxy 已启动（MissionManager + UAVControlLoop 共用）")
|
||
|
||
# 3. 等待收到第一条 Router STATE
|
||
self._wait_for_initial_state()
|
||
|
||
# ---- 启动 TCP 命令通道 ----
|
||
cmd_cfg = self.cfg.get("command_channel", {})
|
||
if cmd_cfg.get("enabled", True):
|
||
self._start_tcp_command_server()
|
||
|
||
# ---- 飞行日志由 FlightLogger 在构造时自动启动 ----
|
||
|
||
self.running = True
|
||
self.logger.info("🚀 所有子系统就绪，主循环启动")
|
||
|
||
def _wait_for_initial_state(self, timeout=5.0):
|
||
"""等待 Router 推送第一条 STATE"""
|
||
deadline = time.time() + timeout
|
||
while time.time() < deadline:
|
||
state = self.proxy.get_latest_state()
|
||
if state is not None:
|
||
drone = state.get("drone", {})
|
||
self.logger.info(
|
||
"✔ 收到 Router 状态: mode=%s armed=%s alt=%.1fm",
|
||
drone.get("mode"), drone.get("armed"),
|
||
drone.get("alt_rel", 0),
|
||
)
|
||
return True
|
||
time.sleep(0.1)
|
||
self.logger.warning("⚠ 未收到 Router 状态 (%ds 超时)，继续启动...", timeout)
|
||
return False
|
||
|
||
# ================================================================
|
||
# 🔁 主循环
|
||
# ================================================================
|
||
|
||
def run(self):
|
||
"""主业务循环（20Hz）"""
|
||
self.logger.info("主循环开始 (20Hz)")
|
||
|
||
prev_state = MissionState.INIT
|
||
loop_count = 0
|
||
|
||
try:
|
||
while self.running:
|
||
loop_start = time.time()
|
||
|
||
# ---- ① 读取视频帧 ----
|
||
ret, frame = self.streamer.read_frame()
|
||
if not ret:
|
||
# 无新帧时短暂等待，不浪费 CPU
|
||
time.sleep(0.005)
|
||
continue
|
||
|
||
loop_count += 1
|
||
orig_shape = frame.shape[:2]
|
||
|
||
# ---- ② NPU 推理 + 后处理 ----
|
||
input_tensor, ratio, dwdh = self.detector.preprocess(frame)
|
||
outputs = self.detector.session.infer([input_tensor])
|
||
detections = self.detector.postprocess(
|
||
outputs, ratio, dwdh, orig_shape,
|
||
)
|
||
self._latest_detections = detections
|
||
target_detected = len(detections) > 0
|
||
|
||
# ---- ③ 控制权协调 ----
|
||
# 根据当前状态机 state 决定 UAVControlLoop 控制线程启停
|
||
current_state = self.mission.state
|
||
self._coordinate_control(current_state, prev_state,
|
||
detections, orig_shape)
|
||
prev_state = current_state
|
||
|
||
# ---- 光流避障决策 ----
|
||
if self.mission.state == MissionState.VISUAL_TRACKING:
|
||
_, _ = self.optical_flow.process_frame(frame)
|
||
yaw_setpoint, speed_scale = self.optical_flow.get_zone_decision()
|
||
self.controller.set_obstacle_decision(yaw_setpoint, speed_scale)
|
||
else:
|
||
# 非追踪状态清除避障
|
||
self.controller.set_obstacle_decision(0.0, 1.0)
|
||
|
||
# ---- ④ 状态机更新 ----
|
||
self.mission.update(target_detected=target_detected)
|
||
|
||
# ---- ⑤ HUD 渲染 + 图传 ----
|
||
frame = self._render_hud(frame, loop_count, detections)
|
||
self.streamer.push_to_stream(frame)
|
||
|
||
# ---- 采集飞行日志 ----
|
||
if self.flight_logger.enabled and self.flight_logger.should_collect():
|
||
self.flight_logger.collect(
|
||
state=self.proxy.get_latest_state(),
|
||
mission_state_name=self.mission.state.name,
|
||
wp_index=self.mission.current_wp_index,
|
||
detect_count=len(self._latest_detections) if self._latest_detections is not None else 0,
|
||
tracking_active=1 if self._control_active else 0,
|
||
)
|
||
|
||
# ---- 频率控制 ----
|
||
elapsed = time.time() - loop_start
|
||
sleep_time = self.loop_interval - elapsed
|
||
if sleep_time > 0:
|
||
time.sleep(sleep_time)
|
||
|
||
except FailsafeTriggered as e:
|
||
# Failsafe 熔断：安全员切出手动模式 → 程序安全退出
|
||
self.logger.warning("🚨 [Failsafe] %s", e)
|
||
except KeyboardInterrupt:
|
||
self.logger.info("⌨ 用户键盘中断，正在安全退出...")
|
||
except Exception as e:
|
||
self.logger.exception("💥 主循环异常: %s", e)
|
||
finally:
|
||
self.shutdown()
|
||
|
||
# ================================================================
|
||
# 控制权协调
|
||
# ================================================================
|
||
|
||
def _coordinate_control(self, current_state, prev_state,
|
||
detections, orig_shape):
|
||
"""
|
||
控制权协调：基于状态机状态启停 UAVControlLoop 线程
|
||
|
||
┌──────────────┬─────────────────────┬──────────────────────┐
|
||
│ 状态机状态 │ MissionManager │ UAVControlLoop │
|
||
├──────────────┼─────────────────────┼──────────────────────┤
|
||
│ 非 TRACKING │ proxy → POSITION │ 控制线程已停止 │
|
||
│ │ SETPOINT │ │
|
||
├──────────────┼─────────────────────┼──────────────────────┤
|
||
│ TRACKING │ 不发 SETPOINT │ 20Hz VELOCITY 线程 │
|
||
│ │ (控制权已让渡) │ TargetTracker + PID │
|
||
└──────────────┴─────────────────────┴──────────────────────┘
|
||
"""
|
||
is_tracking = current_state == MissionState.VISUAL_TRACKING
|
||
was_tracking = prev_state == MissionState.VISUAL_TRACKING
|
||
|
||
if is_tracking:
|
||
# ---- 进入 VISUAL_TRACKING → 启动/保持控制线程 ----
|
||
if not self._control_active:
|
||
self._start_control_thread()
|
||
|
||
# 不断喂入检测数据（TargetTracker 在主线程更新）
|
||
self.controller.update_detections(detections, orig_shape)
|
||
|
||
else:
|
||
# ---- 非 VISUAL_TRACKING → 停止控制线程 ----
|
||
if self._control_active:
|
||
self._stop_control_thread()
|
||
|
||
# 非追踪期间也喂入空检测，保持数据结构新鲜
|
||
self.controller.update_detections([], orig_shape)
|
||
|
||
def _start_control_thread(self):
|
||
"""启动 UAVControlLoop 的 20Hz VELOCITY 控制线程"""
|
||
self.logger.info("▶ [控制权] 启动 UAVControlLoop VELOCITY 线程")
|
||
|
||
# 记录进入追踪时的飞机状态
|
||
state = self.proxy.get_latest_state()
|
||
if state:
|
||
d = state.get("drone", {})
|
||
self.logger.info("追踪初始状态: mode=%s alt=%.1fm armed=%s",
|
||
d.get("mode"), d.get("alt_rel", 0),
|
||
d.get("armed"))
|
||
|
||
# 直接复用 UAVControlLoop 已有的内部 _control_loop 方法
|
||
# （不修改任何内部逻辑，仅从外部启动其线程）
|
||
self.controller.running = True
|
||
self.controller.control_thread = threading.Thread(
|
||
target=self.controller._control_loop,
|
||
daemon=True,
|
||
name="uav-vel-control",
|
||
)
|
||
self.controller.control_thread.start()
|
||
self._control_active = True
|
||
|
||
def _stop_control_thread(self):
|
||
"""安全停止 UAVControlLoop 的控制线程"""
|
||
if not self._control_active:
|
||
return
|
||
|
||
self.logger.info("⏹ [控制权] 停止 UAVControlLoop 线程")
|
||
self.controller.running = False
|
||
|
||
if self.controller.control_thread:
|
||
self.controller.control_thread.join(timeout=2.0)
|
||
self.controller.control_thread = None
|
||
|
||
# 复位 PID 积分项和跟踪器，避免下次启动时积分暴冲
|
||
self.controller.pid_y.reset()
|
||
self.controller.pid_z.reset()
|
||
self.controller.tracker.reset()
|
||
self._control_active = False
|
||
self.logger.info("✔ 控制线程已停止，PID/跟踪器已重置")
|
||
|
||
# ================================================================
|
||
# 渲染
|
||
# ================================================================
|
||
|
||
def _render_hud(self, frame, loop_count, detections):
|
||
"""
|
||
在视频帧上叠加 HUD 信息
|
||
|
||
包含：状态机阶段、航点索引、控制权归属、检测结果
|
||
"""
|
||
# ---- 检测框 ----
|
||
frame = self.detector.draw_boxes(frame, detections)
|
||
|
||
# ---- 状态面板 ----
|
||
state_name = self.mission.state.name
|
||
wp_text = f"WP: {self.mission.current_wp_index + 1}/{len(self.mission.waypoints)}"
|
||
ctrl_text = "CTRL:VELOCITY" if self._control_active else "CTRL:POSITION"
|
||
|
||
cv2.putText(frame, f"State: {state_name} | {wp_text}",
|
||
(20, 40), cv2.FONT_HERSHEY_COMPLEX, 0.6,
|
||
(0, 255, 255), 2)
|
||
cv2.putText(frame, ctrl_text,
|
||
(20, 65), cv2.FONT_HERSHEY_SIMPLEX, 0.5,
|
||
(0, 255, 0) if self._control_active else (255, 255, 0), 2)
|
||
|
||
# ---- 每 100 帧打印详细状态日志 ----
|
||
if loop_count % 100 == 0:
|
||
state = self.proxy.get_latest_state()
|
||
if state:
|
||
d = state.get("drone", {})
|
||
self.logger.info(
|
||
"📡 mode=%s armed=%s alt=%.1fm spd=%.1f bat=%.0f%% "
|
||
"%s %s detect=%d",
|
||
d.get("mode"), d.get("armed"),
|
||
d.get("alt_rel", 0), d.get("ground_speed", 0),
|
||
d.get("battery", 0),
|
||
state_name, ctrl_text,
|
||
len(detections),
|
||
)
|
||
|
||
return frame
|
||
|
||
# ================================================================
|
||
# 🧹 资源释放
|
||
# ================================================================
|
||
|
||
def shutdown(self):
|
||
"""
|
||
安全关闭所有子系统
|
||
|
||
释放顺序（逆初始化顺序）：
|
||
1. 停止控制线程
|
||
2. 关闭 MissionManager 的 ZMQ 代理
|
||
3. 关闭 UAVControlLoop 的 ZMQ 代理
|
||
4. 释放视频流资源
|
||
"""
|
||
self.logger.info("====== 安全关闭所有子系统 ======")
|
||
self.running = False
|
||
|
||
# 1. 停止控制线程
|
||
self._stop_control_thread()
|
||
|
||
# ---- 停止飞行日志线程 ----
|
||
self.flight_logger.stop()
|
||
|
||
# 2. 关闭共享 ZMQ 代理（MissionManager + UAVControlLoop 共用）
|
||
# 注意：UAVControlLoop 不拥有 proxy 生命周期（_owns_proxy=False），
|
||
# 仅在此处统一关闭一次。
|
||
try:
|
||
self.proxy.close()
|
||
self.logger.info("✔ 共享 RouterProxy 已关闭")
|
||
except Exception as e:
|
||
self.logger.warning("proxy close 异常: %s", e)
|
||
|
||
# 3. 释放视频资源
|
||
try:
|
||
self.streamer.release()
|
||
self.logger.info("✔ VideoStreamer 已释放")
|
||
except Exception as e:
|
||
self.logger.warning("streamer release 异常: %s", e)
|
||
|
||
# ---- 停止 TCP 命令通道 ----
|
||
self.tcp_server_running = False
|
||
if self.tcp_server_socket:
|
||
try:
|
||
self.tcp_server_socket.close()
|
||
except Exception:
|
||
pass
|
||
self.tcp_server_socket = None
|
||
if self.tcp_server_thread and self.tcp_server_thread.is_alive():
|
||
self.tcp_server_thread.join(timeout=2.0)
|
||
self.logger.info("✔ TCP 命令通道已关闭")
|
||
|
||
self.logger.info("🏁 所有资源已安全释放，程序退出")
|
||
|
||
|
||
# ================================================================
|
||
# 入口
|
||
# ================================================================
|
||
|
||
def main():
|
||
"""主入口：日志配置 → 编排器创建 → 启动 → 运行"""
|
||
# 创建日志目录
|
||
os.makedirs("./log", exist_ok=True)
|
||
|
||
# 生成带时间戳的日志文件名（精确到秒）
|
||
log_timestamp = time.strftime("%Y%m%d_%H%M%S")
|
||
log_file_path = f"./log/mission_run_{log_timestamp}.log"
|
||
|
||
logging.basicConfig(
|
||
level=logging.INFO,
|
||
format="%(asctime)s - %(name)s - %(levelname)s - %(message)s",
|
||
handlers=[
|
||
logging.StreamHandler(), # 终端输出（保留）
|
||
logging.FileHandler(log_file_path, mode="w"), # 每次新建，覆盖写入
|
||
],
|
||
)
|
||
|
||
# 允许命令行指定配置文件路径
|
||
config_file = sys.argv[1] if len(sys.argv) > 1 else "./config.json"
|
||
# 也可以传入固定点
|
||
# waypoints = [
|
||
# {'x': 1.0, 'y': 0.0, 'z': -1.5, 'yaw': 0},
|
||
# {'x': 1.0, 'y': 1.0, 'z': -1.5, 'yaw': 90},
|
||
# {'x': 0.0, 'y': 0.0, 'z': -1.2, 'yaw': 0},
|
||
# ]
|
||
# orchestrator = MissionOrchestrator(config_path=config_file,waypoints=waypoints)
|
||
|
||
orchestrator = MissionOrchestrator(config_path=config_file,waypoints=[])
|
||
orchestrator.start()
|
||
orchestrator.run()
|
||
|
||
|
||
if __name__ == "__main__":
|
||
main()
|