keita
ded17d7995
- utils/task_runner: 6态状态机(WAIT→ARM_TAKEOFF→GOTO(L2)→TRACK(L1)→APPROACH→RTL) + 编排器(快照采集/状态动作) + 双层日志(事件JSONL/遥测CSV) + 吊舱控制指令(0x07/0x0F/0x0E/0x08) - pod_bridge: ground_link 地面坐标UDP注入(超龄判无效,无激光替代表4); set_heading_servo_enabled() 运行时L1/L2切换(切换即reset清积分防输出跳变); pod_locked 提为公共门控; config 新增 ground_inject 段 - virtual_pod 模拟吊舱(无电机语义) + verify_px4 增量④仅飞控验证 + pod_sitl_test L1 SITL全流程(飞行编排不塞进 run_bridge, 保持纯offboard闭环入口) - 入口: task_runner_sitl(VirtualPod+虚拟目标, SITL端到端跑通约1m24s) / task_runner_real(真串口+UDP注入+吊舱控制) - 地面站 ground_station: 坐标注入 + 状态可视化(Flask) - S1标定回填(2026-10-09): beta_sign=+1 / alpha_sign=-1, 加标定回归测试守护 - 新增测试: task_runner x4 + ground_link/virtual_pod*/bridge/px4_gateway 增量; 全量 483 passed - 文档: 预定流程(需求) + 预定流程-操作指南(操作/待办) + L1方案补S1结论与实现状态
297 lines
13 KiB
Python
297 lines
13 KiB
Python
"""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
|