"""runner.py —— 任务编排器：驱动任务状态机跑通 WAIT→…→RTL 全流程。 「预定流程.md」天空侧的落地：把已通的 L1（航向伺服 TrackingControlLaw）与 L2（地理引导 GuidanceLaw）按任务阶段编排，配合地面坐标注入（GroundLink）与 状态机（TaskStateMachine），串成一条可运行的全流程。 职责边界： * 状态机（TaskStateMachine）只做「状态 + 转移判据」决策； * 本编排器负责「采集快照 → 喂状态机 → 按状态执行动作」； * 闭环控制复用 PodBridge（L1/L2 运行时切换走 set_heading_servo_enabled）； * 飞行编排（解锁/起飞/返航）通过 duck-typed `drone`（MAVSDK System）完成， 便于 mock 单测；真实 SITL 装配见仓库根 task_runner_sitl.py。 生命周期：run() 开头先订阅 PX4 遥测（幂等），用于起飞到位 / 距离计算； GOTO 进入时 start offboard；RTL 进入时安全停机 + 返航。 """ import asyncio import logging import math import time from typing import Callable, Optional from pod_bridge.guidance import haversine_distance_m from visual_track_control.staleness_checker import is_stale from .log import TaskLogger, TelemetryRecord from .state_machine import (TaskInputs, TaskState, TaskStateConfig, TaskStateMachine) logger = logging.getLogger(__name__) def _monotonic_ms() -> int: return int(time.monotonic() * 1000) class TaskRunner: """任务编排器。依赖（除 sm 外均可注入 mock）： cfg BridgeConfig —— ground_inject.timeout_ms 等 sm_cfg TaskStateConfig —— 切换半径/超时/起飞高度/居中阈值/循环频率 px4 Px4Gateway —— own_state()（姿态/位置/相对高度） bridge PodBridge —— step / set_heading_servo_enabled / pod_locked sm TaskStateMachine —— update() ground GroundLink —— latest()（地面注入坐标） drone MAVSDK System —— action.arm/takeoff/return_to_launch/land/disarm vision_fn callable->bool —— 我方视觉第二判据（可选，require_vision 时用） """ def __init__(self, cfg, sm_cfg: TaskStateConfig, px4, bridge, sm: TaskStateMachine, ground, drone, vision_fn: Optional[Callable[[], bool]] = None, now_ms_fn: Callable[[], int] = _monotonic_ms, logger: Optional[TaskLogger] = None, pod_control=None) -> None: self._cfg = cfg self._sm_cfg = sm_cfg self._px4 = px4 self._bridge = bridge self._sm = sm self._ground = ground self._drone = drone self._vision_fn = vision_fn self._now = now_ms_fn self._logger = logger # 结构化任务日志（可选，B 阶段） self._pod_control = pod_control # 吊舱识别/锁定控制（可选，真机） self._centered_since_ms: Optional[int] = None self._rtl_requested = False self._emergency = False self._rtl_done = False # --- 外部控制 ----------------------------------------------------------- def request_rtl(self) -> None: """外部请求返航（如收到地面站返航命令 / 人工按键）。""" self._rtl_requested = True def trigger_emergency(self) -> None: """外部触发紧急终止（丢控 / RC 接管 / 失控）。""" self._emergency = True @property def state(self) -> TaskState: return self._sm.state @property def rtl_done(self) -> bool: return self._rtl_done # --- 主循环 ------------------------------------------------------------- async def run(self, stop_event: asyncio.Event) -> None: """主循环：采集快照 → 状态机 → 进入动作/持续动作，直到 RTL 完成或停止。""" await self._px4.start_telemetry() # 幂等：先拿遥测，供起飞到位/距离计算 if self._logger is not None: self._logger.open() period_s = 1.0 / self._sm_cfg.loop_frequency_hz try: while not stop_event.is_set(): now_ms = self._now() inputs = self._collect_inputs(now_ms) tr = self._sm.update(inputs, now_ms) if tr.occurred: logger.info("[Task] %s -> %s (%s)", tr.from_state.value, tr.to_state.value, tr.reason) if self._logger is not None: self._logger.log_event( now_ms, "state", from_state=tr.from_state.value, to_state=tr.to_state.value, reason=tr.reason) await self._on_enter(tr.to_state, now_ms) out = await self._tick(now_ms) if self._logger is not None: self._logger.log_telemetry( now_ms, self._build_telemetry_record(now_ms, inputs, out)) if self._finished(): break await asyncio.sleep(period_s) finally: await self._teardown() if self._logger is not None: self._logger.close() # --- 输入采集 ----------------------------------------------------------- def _collect_inputs(self, now_ms: int) -> TaskInputs: """聚合 PX4 / 注入坐标 / 吊舱 / 视觉为一份 TaskInputs 快照。""" own = self._px4.own_state() injected = self._ground.latest() target_injected = bool( injected.time_ms > 0 and not is_stale(injected.time_ms, now_ms, self._cfg.ground_inject.timeout_ms)) distance = math.inf if target_injected and own.position_valid: distance = haversine_distance_m( own.latitude_deg, own.longitude_deg, injected.latitude_deg, injected.longitude_deg) takeoff_reached = (own.relative_altitude_m >= self._sm_cfg.takeoff_altitude_m) target_locked = self._bridge.pod_locked(now_ms) vision = bool(self._vision_fn and self._vision_fn()) centered, centered_s = self._update_centered(now_ms) return TaskInputs( target_injected=target_injected, distance_to_target_m=distance, takeoff_reached=takeoff_reached, target_locked=target_locked, vision_has_target=vision, target_centered=centered, centered_duration_s=centered_s, rtl_requested=self._rtl_requested, emergency=self._emergency, ) def _update_centered(self, now_ms: int) -> tuple[bool, float]: """脱靶量居中判定 + 持续时长累加（TRACK→APPROACH 判据）。 无遥测 / 任一轴脱靶量超出 center_threshold_deg → 清零累加。 """ tel = self._bridge.latest_telemetry if tel is None: self._centered_since_ms = None return False, 0.0 frame, _ = tel centered = (abs(frame.off_target_x_deg) < self._sm_cfg.center_threshold_deg and abs(frame.off_target_y_deg) < self._sm_cfg.center_threshold_deg) if not centered: self._centered_since_ms = None return False, 0.0 if self._centered_since_ms is None: self._centered_since_ms = now_ms return True, (now_ms - self._centered_since_ms) / 1000.0 # --- 状态进入动作 ------------------------------------------------------- async def _on_enter(self, state: TaskState, now_ms: int) -> None: if state is TaskState.ARM_TAKEOFF: await self._arm_takeoff() elif state is TaskState.GOTO: if not await self._bridge.start(): logger.error("[Task] offboard 启动失败，紧急终止") self._emergency = True return self._bridge.set_heading_servo_enabled(False) # L2 地理引导 if self._pod_control is not None: self._pod_control.arm_detection() # 真机：开启目标识别 + 自动锁定 elif state is TaskState.TRACK: self._bridge.set_heading_servo_enabled(True) # L1 航向伺服 elif state is TaskState.APPROACH: pass # 悬停由 _tick 持续发零速 elif state is TaskState.RTL: if self._pod_control is not None: self._pod_control.stop_tracking() # 真机：停止跟踪 + 识别关（bridge 停机前） await self._start_rtl() # --- 每周期持续动作 ----------------------------------------------------- async def _tick(self, now_ms: int): """每周期持续动作。返回本拍控制输出（GOTO/TRACK 有，其余 None）。""" state = self._sm.state if state in (TaskState.GOTO, TaskState.TRACK): return await self._bridge.step(now_ms) elif state is TaskState.APPROACH: await self._px4.send_brake_hover(now_ms) # 悬停目标上空（保 offboard 心跳） elif state is TaskState.RTL: await self._check_landed() return None # WAIT / ARM_TAKEOFF：无持续动作（待命 / 等起飞高度） def _build_telemetry_record(self, now_ms: int, inputs: TaskInputs, out) -> TelemetryRecord: """聚合本拍遥测为一帧 TelemetryRecord（供日志 / 页面复用）。""" own = self._px4.own_state() tel = self._bridge.latest_telemetry off_x = off_y = 0.0 if tel is not None: frame, _ = tel off_x = frame.off_target_x_deg off_y = frame.off_target_y_deg return TelemetryRecord( state=self._sm.state.value, pos_valid=own.position_valid, fix_type=own.fix_type, num_satellites=own.num_satellites, latitude_deg=own.latitude_deg, longitude_deg=own.longitude_deg, relative_altitude_m=own.relative_altitude_m, yaw_deg=math.degrees(own.yaw_rad), target_injected=inputs.target_injected, distance_to_target_m=inputs.distance_to_target_m, target_locked=inputs.target_locked, off_target_x_deg=off_x, off_target_y_deg=off_y, mode=out.mode.name if out is not None else "", yaw_rate_dps=out.yaw_rate_dps if out is not None else 0.0, vx_body_mps=out.vx_body_mps if out is not None else 0.0, vz_mps=out.vz_mps if out is not None else 0.0, ) # --- 飞行编排 ----------------------------------------------------------- async def _arm_takeoff(self) -> None: try: await self._drone.action.arm() await self._drone.action.set_takeoff_altitude( self._sm_cfg.takeoff_altitude_m) await self._drone.action.takeoff() if self._logger is not None: self._logger.log_event( self._now(), "takeoff", altitude_m=self._sm_cfg.takeoff_altitude_m) except Exception as exc: logger.error("[Task] 起飞失败: %s", exc) self._emergency = True if self._logger is not None: self._logger.log_event(self._now(), "takeoff_failed", error=str(exc)) async def _start_rtl(self) -> None: try: await self._bridge.stop() # 先零速刹车 + 停 offboard/遥测/串口 except Exception as exc: logger.warning("[Task] 停机异常: %s", exc) try: await self._drone.action.return_to_launch() except Exception as exc: logger.warning("[Task] RTL 失败，尝试降落: %s", exc) try: await self._drone.action.land() except Exception: pass if self._logger is not None: self._logger.log_event(self._now(), "rtl") async def _check_landed(self) -> None: """RTL 状态每周期取一帧 in_air 遥测；落地后 disarm 并标记完成。""" try: async for is_in_air in self._drone.telemetry.in_air(): if not is_in_air and not self._rtl_done: self._rtl_done = True try: await self._drone.action.disarm() except Exception: pass break # 只取最新一帧 except Exception: pass # --- 收尾 --------------------------------------------------------------- def _finished(self) -> bool: return self._sm.state is TaskState.RTL and self._rtl_done async def _teardown(self) -> None: """退出主循环（含异常/中途停止）时的安全收尾。""" try: await self._bridge.stop() except Exception: pass