diff --git a/infer_camera_modular.py b/infer_camera_modular.py index b344add..d4747d5 100644 --- a/infer_camera_modular.py +++ b/infer_camera_modular.py @@ -287,18 +287,24 @@ class UAVControlLoop: - 无目标时 PID 自动复位 → 输出零速 """ - def __init__(self, fc_cfg, pid_y_cfg, pid_z_cfg): + def __init__(self, fc_cfg, pid_y_cfg, pid_z_cfg, proxy=None): self.logger = logging.getLogger("UAVControlLoop") # ---- 保存 config（供 start_uav 使用如 takeoff_alt 等） ---- self.fc_cfg = fc_cfg # ----------------------------------------------------------- - # ---- 使用 RouterProxy 替换 DroneController ---- - self.proxy = RouterProxy( - req_endpoint=fc_cfg.get("req_endpoint", "tcp://127.0.0.1:5555"), - sub_endpoint=fc_cfg.get("sub_endpoint", "tcp://127.0.0.1:5556") - ) + # ---- 使用 RouterProxy（支持外部注入共享实例） ---- + if proxy is not None: + self.proxy = proxy + self._owns_proxy = False + self.logger.info("使用外部注入的共享 RouterProxy") + else: + self.proxy = RouterProxy( + req_endpoint=fc_cfg.get("req_endpoint", "tcp://127.0.0.1:5555"), + sub_endpoint=fc_cfg.get("sub_endpoint", "tcp://127.0.0.1:5556") + ) + self._owns_proxy = True # --------------------------------------------- self.pid_y = PID(**pid_y_cfg) @@ -679,8 +685,12 @@ class UAVControlLoop: self.tracker.reset() self._tracked_result = None - self.proxy.close() - self.logger.info("控制已停止，ZMQ 代理已关闭，跟踪器已重置") + if self._owns_proxy: + self.proxy.close() + self.logger.info("自有 RouterProxy 已关闭") + else: + self.logger.info("共享 RouterProxy 交由外部管理，跳过 close") + self.logger.info("控制已停止，跟踪器已重置") # ==================== 主程序入口 ====================== diff --git a/run_mission.py b/run_mission.py index a536bf1..7082e23 100644 --- a/run_mission.py +++ b/run_mission.py @@ -5,19 +5,21 @@ run_mission.py — 基于 ZMQ Router 协议的主任务入口 本脚本是整个无人机任务系统的总装粘合剂。采用领域驱动模块化设计， 将各子系统按职责独立实例化，在主循环中进行控制权协调。 - ┌─ MissionManager ────────────┐ - │ RouterProxy (own) │ 状态机管理 + 航点巡航 - │ POSITION SETPOINT (非追踪) │ 控制权：NAVIGATING/HOLD_TASK/... - └─────────────────────────────┘ - ↕ 控制权协调 - ┌─ UAVControlLoop ────────────┐ - │ RouterProxy (own) │ 20Hz VELOCITY 闭环追踪 - │ TargetTracker + PID │ 控制权：仅 VISUAL_TRACKING - └─────────────────────────────┘ - ┌─ 感知模块 ───────────────────┐ - │ VideoStreaming │ 摄像头/RTSP 采集 - │ YOLO26UAVInfer │ 昇腾 NPU 推理 - └─────────────────────────────┘ + ┌─ MissionOrchestrator ──────────┐ + │ RouterProxy (shared) │ 单一共享 ZMQ 代理 + └──────────┬─────────────────────┘ + │ 注入 + ┌─────┴──────┐ + ▼ ▼ + ┌─ MissionManager ┌─ UAVControlLoop ──┐ + │ 状态机 + 航点巡航 │ 20Hz VELOCITY │ + │ POSITION SETPOINT │ 闭环追踪 │ + │ (非追踪期间) │ (VISUAL_TRACKING) │ + └───────────────────┴────────────────────┘ + ┌─ 感知模块 ─────────┐ + │ VideoStreaming │ 摄像头/RTSP 采集 + │ YOLO26UAVInfer │ 昇腾 NPU 推理 + └───────────────────┘ 【控制权协调规则】 同一时刻只有一个人持有「SETPOINT 发令权」： @@ -27,7 +29,8 @@ run_mission.py — 基于 ZMQ Router 协议的主任务入口 【安全设计】 - try / except FailsafeTriggered / finally 全面资源释放 - - 两个独立 RouterProxy，避免 REQ socket 线程竞争 + - MissionManager 和 UAVControlLoop 共享同一 RouterProxy， + 通过控制权协调避免 REQ socket 并发冲突 - 控制线程启停由状态机状态变化驱动，逻辑可预测 【可扩展的图像输入接口】 @@ -107,18 +110,17 @@ class MissionOrchestrator: # ================================================================ # 🚁 MissionManager（状态机 + 航点巡航） # ================================================================ - # 拥有独立的 RouterProxy，用于读取 STATE 和发送 COMMAND / - # WAYPOINT / POSITION SETPOINT。 + # 与 UAVControlLoop 共享同一个 RouterProxy（见下方）。 fc = cfg["flight_control"] - self.mission_proxy = RouterProxy( + 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.mission_proxy.set_waypoints_callback(self._on_qgc_waypoints) + self.proxy.set_waypoints_callback(self._on_qgc_waypoints) self.mission = MissionManager( - proxy=self.mission_proxy, + proxy=self.proxy, waypoints=self.waypoints, target_altitude=1.5, arrival_radius=0.3, @@ -129,10 +131,11 @@ class MissionOrchestrator: # ================================================================ # 🛸 UAVControlLoop（20Hz VELOCITY 闭环追踪） # ================================================================ - # 拥有独立的 RouterProxy。其 20Hz 控制线程仅在 VISUAL_TRACKING - # 期间运行，其余时间处于停止状态，避免与 MissionManager 冲突。 + # 注入与 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 # 控制线程运行标志 @@ -247,7 +250,7 @@ class MissionOrchestrator: def _collect_and_enqueue_log(self): """采集当前飞行状态，推入日志队列（主线程执行，极轻量）""" try: - state = self.mission_proxy.get_latest_state() + state = self.proxy.get_latest_state() if state is None: return @@ -269,7 +272,7 @@ class MissionOrchestrator: drone.get("battery", 0.0), # battery_percent current_state, # state (string) wp_idx, # wp_index - len(self._latest_detections) if hasattr(self, 'detections') else 0, # detect_count + len(self._latest_detections) if self._latest_detections is not None else 0, # detect_count tracking_active # tracking_active ) @@ -300,7 +303,7 @@ class MissionOrchestrator: self.logger.info("📥 收到 %d 个航点，开始坐标转换...", len(raw_wps)) # ---- ① 获取参考点（起飞点） ---- - state = self.mission_proxy.get_latest_state() + state = self.proxy.get_latest_state() if state is None: self.logger.error("无法获取 STATE，缺少参考经纬度，航点转换失败") return @@ -488,15 +491,9 @@ class MissionOrchestrator: """启动所有子系统""" self.logger.info("正在启动所有子系统...") - # 1. 启动 MissionManager 的 ZMQ 代理（SUB 监听 + REQ 就绪） - self.mission_proxy.start() - self.logger.info("✔ MissionManager Proxy 已启动") - - # 2. 启动 UAVControlLoop 的 ZMQ 代理 - # start_uav() 会执行完整的起飞序列，我们不需要 —— - # MissionManager 负责起飞。这里只启动 SUB 线程收 STATE。 - self.controller.proxy.start() - self.logger.info("✔ UAVControlLoop Proxy 已启动") + # 1. 启动共享 ZMQ 代理（MissionManager 与 UAVControlLoop 共用） + self.proxy.start() + self.logger.info("✔ 共享 RouterProxy 已启动（MissionManager + UAVControlLoop 共用）") # 3. 等待收到第一条 Router STATE self._wait_for_initial_state() @@ -516,7 +513,7 @@ class MissionOrchestrator: """等待 Router 推送第一条 STATE""" deadline = time.time() + timeout while time.time() < deadline: - state = self.mission_proxy.get_latest_state() + state = self.proxy.get_latest_state() if state is not None: drone = state.get("drone", {}) self.logger.info( @@ -642,7 +639,7 @@ class MissionOrchestrator: self.logger.info("▶ [控制权] 启动 UAVControlLoop VELOCITY 线程") # 记录进入追踪时的飞机状态 - state = self.mission_proxy.get_latest_state() + state = self.proxy.get_latest_state() if state: d = state.get("drone", {}) self.logger.info("追踪初始状态: mode=%s alt=%.1fm armed=%s", @@ -694,7 +691,7 @@ class MissionOrchestrator: # ---- 状态面板 ---- state_name = self.mission.state.name - wp_text = f"WP: {self.mission.current_wp_index + 1}/{len(self.waypoints)}" + 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}", @@ -706,7 +703,7 @@ class MissionOrchestrator: # ---- 每 100 帧打印详细状态日志 ---- if loop_count % 100 == 0: - state = self.mission_proxy.get_latest_state() + state = self.proxy.get_latest_state() if state: d = state.get("drone", {}) self.logger.info( @@ -754,21 +751,16 @@ class MissionOrchestrator: self._log_thread.join(timeout=2.0) self.logger.info("✔ 飞行日志线程已停止") - # 2. 关闭 MissionManager 的 Proxy + # 2. 关闭共享 ZMQ 代理（MissionManager + UAVControlLoop 共用） + # 注意：UAVControlLoop 不拥有 proxy 生命周期（_owns_proxy=False）， + # 仅在此处统一关闭一次。 try: - self.mission_proxy.close() - self.logger.info("✔ MissionManager Proxy 已关闭") + self.proxy.close() + self.logger.info("✔ 共享 RouterProxy 已关闭") except Exception as e: - self.logger.warning("mission_proxy close 异常: %s", e) + self.logger.warning("proxy close 异常: %s", e) - # 3. 关闭 UAVControlLoop 的 Proxy（直接访问其内部 proxy） - try: - self.controller.proxy.close() - self.logger.info("✔ UAVControlLoop Proxy 已关闭") - except Exception as e: - self.logger.warning("control_proxy close 异常: %s", e) - - # 4. 释放视频资源 + # 3. 释放视频资源 try: self.streamer.release() self.logger.info("✔ VideoStreamer 已释放")