824 lines
32 KiB
Python
824 lines
32 KiB
Python
"""
|
||
mission_manager.py — 基于 ZeroMQ RouterProxy 的航点巡航状态机
|
||
|
||
【重构背景】
|
||
系统底层架构已升级为 ZMQ Router 代理通信协议。本模块彻底摒弃
|
||
旧版 DroneController 直连方式，全面迁移至 RouterProxy。
|
||
|
||
【核心改动】
|
||
1. self.drone → self.proxy（注入的 RouterProxy 实例）
|
||
2. 所有状态读取：proxy.get_latest_state()["drone"][...]
|
||
3. 所有指令发送：proxy.send_command() / send_waypoint() / send_setpoint()
|
||
4. 新增 STATE 新鲜度校验（协议 §3.8.1），过期数据保守处理
|
||
5. 新增 Failsafe 模式断流保护，退出 OFFBOARD 即熔断
|
||
6. VISUAL_TRACKING 状态下完全让渡控制权，不发送任何 SETPOINT
|
||
|
||
【位置估计】
|
||
Router 协议未暴露局部 NED 坐标，本模块使用速度积分
|
||
（vx × dt 累加）推算相对起飞点的水平位置，用于航点到
|
||
达判定。垂直轴直接使用 state["drone"]["alt_rel"]。
|
||
|
||
【控制权协调】
|
||
MissionManager 通过自有 proxy 发送指令。连续 20Hz SETPOINT
|
||
流由 UAVControlLoop 的独立线程负责。run_mission.py 负责协调：
|
||
在 VISUAL_TRACKING 外停止 UAVControlLoop 线程，避免两套
|
||
SETPOINT 流同时激活产生冲突。
|
||
"""
|
||
|
||
import time
|
||
import math
|
||
import logging
|
||
from enum import Enum
|
||
|
||
from sympy import true
|
||
|
||
logger = logging.getLogger(__name__)
|
||
|
||
|
||
class MissionState(Enum):
|
||
"""无人机任务状态枚举"""
|
||
WAITING_WAYPOINTS=-1 # 等待地面站下发航点
|
||
INIT = 0 # 初始化 / 等待连接
|
||
ARMING = 1 # 解锁中
|
||
TAKEOFF = 2 # 自动起飞中
|
||
NAVIGATING = 3 # 航点巡航导航
|
||
HOLD_TASK = 4 # 到达航点后的原地搜索 / 悬停
|
||
VISUAL_TRACKING = 5 # 🎯 视觉追踪中（控制权让渡给 UAVControlLoop）
|
||
RETURNING = 6 # 返航中
|
||
LANDING = 7 # 自动降落中
|
||
FINISHED = 8 # 任务结束 / 已上锁
|
||
HOLD_FINAL = 9 # 结束巡点后原地悬停（return_to_home=False）
|
||
|
||
|
||
class FailsafeTriggered(Exception):
|
||
"""
|
||
自定义安全异常：当安全员人工介入切出自控模式时抛出，
|
||
用于暴力熔断主循环。
|
||
"""
|
||
pass
|
||
|
||
|
||
# ================================================================
|
||
# 内部工具
|
||
# ================================================================
|
||
|
||
def _dist_3d(x1, y1, z1, x2, y2, z2):
|
||
"""三维欧氏距离"""
|
||
return math.sqrt((x1 - x2) ** 2 + (y1 - y2) ** 2 + (z1 - z2) ** 2)
|
||
|
||
|
||
# ================================================================
|
||
# 航点巡航管理器
|
||
# ================================================================
|
||
|
||
class MissionManager:
|
||
"""
|
||
🛸 ZMQ 航点巡航状态机管理器
|
||
|
||
职责：
|
||
- 封装完整的无人机任务生命周期（初始化 → 解锁 → 起飞 →
|
||
航点巡航 → 视觉追踪 → 返航 → 降落）
|
||
- 所有底层通信通过注入的 RouterProxy 完成
|
||
- 在 VISUAL_TRACKING 状态下让渡控制权给 UAVControlLoop
|
||
- Failsafe 熔断保护
|
||
|
||
使用示例：
|
||
proxy = RouterProxy()
|
||
proxy.start()
|
||
manager = MissionManager(proxy, waypoints=[...], target_altitude=1.5)
|
||
|
||
while True:
|
||
manager.update(target_detected=some_bool)
|
||
time.sleep(0.05)
|
||
"""
|
||
|
||
# ---- Failsafe：自主飞行的合法模式白名单 ----
|
||
AUTONOMOUS_MODES = frozenset({
|
||
"OFFBOARD", "GUIDED", "AUTO", "HOLD", "LAND", "RTL",
|
||
})
|
||
|
||
# ---- 位置估计：速度积分有效时间窗口 ----
|
||
MAX_POSITION_DT = 1.0
|
||
|
||
# ---- 目标丢失超时（秒） ----
|
||
DEFAULT_TARGET_LOST_TIMEOUT = 3.0
|
||
|
||
# ---- 各阶段超时 ----
|
||
TAKEOFF_TIMEOUT = 15.0
|
||
OFFBOARD_TIMEOUT = 5.0
|
||
ARM_TIMEOUT = 5.0
|
||
|
||
def __init__(self, proxy, waypoints, target_altitude=1.5,
|
||
arrival_radius=0.3, hold_duration=5.0,
|
||
return_to_home=True, target_lost_timeout=None):
|
||
"""
|
||
Args:
|
||
proxy: RouterProxy 实例（调用方负责 start/close）
|
||
waypoints: 航点列表 [{'x':1,'y':2,'z':-1.5,'yaw':0}, ...]
|
||
target_altitude: 默认起飞高度（米，正值=向上）
|
||
arrival_radius: 航点到达判定半径（米）
|
||
hold_duration: 巡点无目标的最大悬停搜索时间（秒）
|
||
return_to_home: 航点巡完后是否返航，False 则终点悬停
|
||
target_lost_timeout: VISUAL_TRACKING 目标丢失超时（秒）
|
||
"""
|
||
# ---- 依赖注入 ----
|
||
self.proxy = proxy
|
||
|
||
# ---- 任务参数 ----
|
||
# 航点列表（外部队列已经使用 NED 坐标系，Z 负值向上）
|
||
self.waypoints = list(waypoints) if waypoints else []
|
||
# 根据是否有航点决定初始状态
|
||
if self.waypoints:
|
||
# 有预置航点（如测试用硬编码），直接进入初始化
|
||
self.state = MissionState.INIT
|
||
self._waypoints_ready = True
|
||
logger.info("MissionManager 初始化完成，已有 %d 个航点", len(self.waypoints))
|
||
else:
|
||
# 无航点，进入等待状态（等待 QGC 下发）
|
||
self.state = MissionState.WAITING_WAYPOINTS
|
||
self._waypoints_ready = False
|
||
logger.info("MissionManager 初始化完成，等待 QGC 下发航点...")
|
||
self.target_altitude = -abs(target_altitude) # NED Z（负值向上）
|
||
self.arrival_radius = arrival_radius
|
||
self.hold_duration = hold_duration
|
||
self.return_to_home = return_to_home
|
||
self.target_lost_timeout = (
|
||
target_lost_timeout if target_lost_timeout is not None
|
||
else self.DEFAULT_TARGET_LOST_TIMEOUT
|
||
)
|
||
|
||
# ---- 状态机 ----
|
||
self.current_wp_index = 0
|
||
|
||
# ---- 当前目标航点（共享给外部主循环 / UAVControlLoop） ----
|
||
self.current_target = {
|
||
"x": 0.0, "y": 0.0,
|
||
"z": self.target_altitude, "yaw": 0.0,
|
||
}
|
||
|
||
# ================================================================
|
||
# 本地位置估计（速度积分法）
|
||
# ================================================================
|
||
self._est_x = 0.0 # NED 北向（相对起飞点）
|
||
self._est_y = 0.0 # NED 东向
|
||
self._pos_time = 0.0 # 上次位置更新时间戳
|
||
self._home_set = False # 首次 STATE 标记
|
||
|
||
# ---- 计时器 ----
|
||
self.hold_start_time = 0.0
|
||
self.tracking_start_time = 0.0
|
||
self._target_lost_start = None
|
||
self._state_start_time = time.time()
|
||
|
||
# ================================================================
|
||
# 追踪完成信号（外部设置）
|
||
# ================================================================
|
||
# UAVControlLoop 或 run_mission.py 在判定视觉追踪完成时置 True
|
||
self.tracking_completed = False
|
||
# 追踪最大时长兜底
|
||
self.tracking_max_duration = 10.0
|
||
|
||
logger.info("MissionManager 初始化: %d 个航点, 目标高度=%.1fm, "
|
||
"到达半径=%.1fm, 搜寻时长=%.1fs, 返航=%s",
|
||
len(self.waypoints), -self.target_altitude,
|
||
self.arrival_radius, self.hold_duration,
|
||
self.return_to_home)
|
||
|
||
# ================================================================
|
||
# 主入口
|
||
# ================================================================
|
||
|
||
def update(self, target_detected=False):
|
||
"""
|
||
状态机高频核心业务逻辑，由外部主循环驱动（建议 ≥20Hz）
|
||
|
||
Args:
|
||
target_detected: 当前帧视觉检测是否有目标
|
||
|
||
Raises:
|
||
FailsafeTriggered: 检测到非法模式切换时抛出
|
||
"""
|
||
# ---------------------------------------------------------------
|
||
# 步骤 0：获取最新 STATE + 新鲜度校验 + Failsafe
|
||
# ---------------------------------------------------------------
|
||
state = self.proxy.get_latest_state()
|
||
|
||
# ---- 新鲜度校验（协议 §3.8.1） ----
|
||
if self.state not in (MissionState.INIT, MissionState.FINISHED):
|
||
if state is not None:
|
||
if not self.proxy.is_state_fresh(state):
|
||
age_us, _ = self.proxy.get_state_freshness(state)
|
||
age_s = (age_us or 0) / 1_000_000
|
||
logger.warning("⚠️ STATE 严重过期 (age=%.2fs)，"
|
||
"本轮保守跳过", age_s)
|
||
return
|
||
else:
|
||
logger.warning("⚠️ 尚未收到 STATE，跳过本轮处理")
|
||
return
|
||
|
||
# ---- 特殊状态，等待航点 ----
|
||
if self.state == MissionState.WAITING_WAYPOINTS:
|
||
if self._waypoints_ready:
|
||
logger.info("✅ 航点已就绪，转入 INIT 状态")
|
||
self.state = MissionState.INIT
|
||
self._state_start_time = time.time()
|
||
else:
|
||
return
|
||
|
||
# ---- Failsafe 越权检查 ----
|
||
if self.state not in (MissionState.INIT, MissionState.FINISHED):
|
||
if state is not None:
|
||
mode = state["drone"].get("mode", "")
|
||
if mode not in self.AUTONOMOUS_MODES:
|
||
logger.warning(
|
||
"\n⚠️ [Failsafe] 飞控模式被切为: %s！"
|
||
"人工接管，熔断任务！", mode
|
||
)
|
||
self.state = MissionState.FINISHED
|
||
raise FailsafeTriggered(
|
||
f"安全员接管，当前模式: {mode}"
|
||
)
|
||
|
||
# ---- 无 STATE 时仅 INIT 可容忍 ----
|
||
if state is None:
|
||
if self.state == MissionState.INIT:
|
||
logger.info("[INIT] 等待 Router 状态推送...")
|
||
return
|
||
|
||
# ---------------------------------------------------------------
|
||
# 步骤 1：更新本地位置估计
|
||
# ---------------------------------------------------------------
|
||
self._update_local_position(state)
|
||
|
||
# ---------------------------------------------------------------
|
||
# 步骤 2：状态机分支
|
||
# ---------------------------------------------------------------
|
||
try:
|
||
if self.state == MissionState.INIT:
|
||
self._handle_init(state)
|
||
elif self.state == MissionState.ARMING:
|
||
self._handle_arming(state)
|
||
elif self.state == MissionState.TAKEOFF:
|
||
self._handle_takeoff(state)
|
||
elif self.state == MissionState.NAVIGATING:
|
||
self._handle_navigating(state)
|
||
elif self.state == MissionState.HOLD_TASK:
|
||
self._handle_hold_task(state, target_detected)
|
||
elif self.state == MissionState.VISUAL_TRACKING:
|
||
self._handle_visual_tracking(state, target_detected)
|
||
elif self.state == MissionState.RETURNING:
|
||
self._handle_returning(state)
|
||
elif self.state == MissionState.LANDING:
|
||
self._handle_landing(state)
|
||
elif self.state == MissionState.HOLD_FINAL:
|
||
self._handle_hold_final(state)
|
||
except FailsafeTriggered:
|
||
raise
|
||
except Exception as e:
|
||
logger.error("状态机异常 (state=%s): %s", self.state.name, e,
|
||
exc_info=True)
|
||
|
||
# ================================================================
|
||
# 位置估计
|
||
# ================================================================
|
||
|
||
def _update_local_position(self, state):
|
||
"""通过速度积分更新本地 NED 位置估计"""
|
||
try:
|
||
drone = state["drone"]
|
||
now = time.time()
|
||
|
||
if not self._home_set:
|
||
self._est_x = 0.0
|
||
self._est_y = 0.0
|
||
self._pos_time = now
|
||
self._home_set = True
|
||
logger.info("[位置] 初始位置记录完成")
|
||
|
||
dt = now - self._pos_time
|
||
if 0 < dt < self.MAX_POSITION_DT:
|
||
vx = drone.get("vx", 0.0)
|
||
vy = drone.get("vy", 0.0)
|
||
self._est_x += vx * dt
|
||
self._est_y += vy * dt
|
||
# dt ≥ MAX_POSITION_DT 时跳过积分防跳变
|
||
|
||
self._pos_time = now
|
||
except (KeyError, TypeError) as e:
|
||
logger.warning("[位置] 更新异常: %s", e)
|
||
|
||
def _get_current_z(self, state):
|
||
"""从 STATE 获取当前 NED Z（负值向上）"""
|
||
try:
|
||
return -state["drone"].get("alt_rel", 0.0)
|
||
except (KeyError, TypeError):
|
||
return self.target_altitude
|
||
|
||
# ================================================================
|
||
# 状态处理器
|
||
# ================================================================
|
||
|
||
# ---- 初始化 ----
|
||
|
||
def _handle_init(self, state):
|
||
"""
|
||
[INIT] 等待飞控连接，判断是否需要执行完整起飞序列
|
||
|
||
三种分支：
|
||
A) 已解锁 + OFFBOARD → 直接跳 NAVIGATING
|
||
B) 未解锁 → 发 ARM 指令
|
||
C) 已解锁但非 OFFBOARD → 进入 ARMING 等待后续 TAKEOFF 序列
|
||
"""
|
||
global need_homeLog
|
||
try:
|
||
drone = state["drone"]
|
||
armed = drone.get("armed", False)
|
||
mode = drone.get("mode", "")
|
||
alt_rel = drone.get("alt_rel", 0.0)
|
||
home = state.get("home", {})
|
||
|
||
logger.info("[INIT] mode=%s armed=%s alt_rel=%.1fm",
|
||
mode, armed, alt_rel)
|
||
|
||
if not home.get("valid", False) and need_homeLog:
|
||
logger.warning("[INIT] home.valid=false，等待起飞点有效...")
|
||
need_homeLog = False
|
||
return
|
||
|
||
# A) 已在 OFFBOARD + 已解锁 → 直接巡航
|
||
if armed and mode == "OFFBOARD":
|
||
logger.info("[INIT] 已在 OFFBOARD，直接进入航点巡航")
|
||
self.state = MissionState.NAVIGATING
|
||
self.current_wp_index = 0
|
||
self._set_current_target_from_wp(0)
|
||
return
|
||
|
||
# B) 未解锁
|
||
if not armed:
|
||
logger.info("[INIT] 执行解锁...")
|
||
ok, ack = self.proxy.send_command("ARM")
|
||
if ok:
|
||
self.state = MissionState.ARMING
|
||
self._state_start_time = time.time()
|
||
else:
|
||
logger.error("[INIT] ARM 失败: %s", ack)
|
||
return
|
||
|
||
# C) 已解锁但非 OFFBOARD
|
||
if armed and mode != "OFFBOARD":
|
||
logger.info("[INIT] 已解锁但 mode=%s，进入起飞序列", mode)
|
||
self.state = MissionState.ARMING
|
||
self._state_start_time = time.time()
|
||
|
||
except (KeyError, TypeError) as e:
|
||
logger.warning("[INIT] 解析异常: %s", e)
|
||
|
||
# ---- 解锁 ----
|
||
|
||
def _handle_arming(self, state):
|
||
"""
|
||
[ARMING] 等待解锁确认，成功后进入 TAKEOFF
|
||
"""
|
||
global need_homeLog
|
||
try:
|
||
armed = state["drone"].get("armed", False)
|
||
home = state.get("home", {})
|
||
need_homeLog = True
|
||
|
||
if not armed:
|
||
elapsed = time.time() - self._state_start_time
|
||
if elapsed > self.ARM_TIMEOUT:
|
||
logger.warning("[ARMING] 超时 (%.1fs)，重试 ARM", elapsed)
|
||
self.proxy.send_command("ARM")
|
||
self._state_start_time = time.time()
|
||
else:
|
||
logger.info("[ARMING] 等待解锁... (%.1fs)", elapsed)
|
||
return
|
||
|
||
if not home.get("valid", False) and need_homeLog:
|
||
logger.warning("[ARMING] home.valid=false，等待起飞点有效...")
|
||
need_homeLog = False
|
||
return
|
||
|
||
# ---- 解锁成功 → 起飞 ----
|
||
takeoff_alt = -self.target_altitude # 正值（协议 altitude 为正）
|
||
logger.info("\n[解锁成功] 发送起飞指令 (alt=%.1fm)", takeoff_alt)
|
||
|
||
ok, ack = self.proxy.send_waypoint(
|
||
lat=state["home"]["lat"],
|
||
lon=state["home"]["lon"],
|
||
alt=takeoff_alt,
|
||
alt_frame="RELATIVE",
|
||
speed=3.0, action="TAKEOFF",
|
||
yaw=state["drone"]["yaw"],
|
||
acceptance_radius=0.5,
|
||
is_last=True
|
||
)
|
||
if ok:
|
||
self.state = MissionState.TAKEOFF
|
||
self._state_start_time = time.time()
|
||
else:
|
||
logger.warning("[TAKEOFF] WAYPOINT 失败: %s，尝试 SETPOINT 爬升", ack)
|
||
ok2, _ = self.proxy.send_setpoint(
|
||
x=0, y=0, z=self.target_altitude, yaw=0.0,
|
||
control_mode="POSITION",
|
||
)
|
||
if ok2:
|
||
self.state = MissionState.TAKEOFF
|
||
self._state_start_time = time.time()
|
||
else:
|
||
logger.error("[TAKEOFF] 降级爬升也失败")
|
||
|
||
except (KeyError, TypeError) as e:
|
||
logger.warning("[ARMING] 异常: %s", e)
|
||
|
||
# ---- 起飞 ----
|
||
|
||
def _handle_takeoff(self, state):
|
||
"""
|
||
[TAKEOFF] 持续发送 POSITION SETPOINT 爬升，到达后进入 OFFBOARD
|
||
|
||
流程：
|
||
① 每帧发送 POSITION 悬停/爬升 SETPOINT（预热 + 保持流）
|
||
② 高度到达目标 90% → 发送 OFFBOARD 指令
|
||
③ 模式切换确认 → 跳转到 NAVIGATING
|
||
"""
|
||
try:
|
||
drone = state["drone"]
|
||
alt_rel = drone.get("alt_rel", 0.0)
|
||
mode = drone.get("mode", "")
|
||
target_up = -self.target_altitude # 正值
|
||
|
||
# ---- ① 持续发送 POSITION SETPOINT（预热 OFFBOARD 流） ----
|
||
# 通过 MissionManager 自有 proxy 发送，此阶段 UAVControlLoop
|
||
# 的 20Hz 线程会被 run_mission.py 关闭，不会冲突。
|
||
# NED 坐标系：x=0,y=0,z=target_altitude 表示飞到正上方目标高度
|
||
self.proxy.send_setpoint(
|
||
x=0, y=0, z=self.target_altitude, yaw=0.0,
|
||
control_mode="POSITION",
|
||
)
|
||
|
||
# ---- ② 到达目标高度 → 进入 OFFBOARD ----
|
||
reached_alt = alt_rel >= target_up * 0.9
|
||
|
||
if reached_alt and mode != "OFFBOARD":
|
||
logger.info("[TAKEOFF] 高度 %.1f/%.1f，切换 OFFBOARD...",
|
||
alt_rel, target_up)
|
||
# 先预热悬停（协议 §3.3.1 ③→④）
|
||
self.proxy.send_setpoint(
|
||
x=0, y=0, z=-alt_rel, yaw=0.0,
|
||
control_mode="POSITION",
|
||
)
|
||
time.sleep(0.2)
|
||
|
||
ok, ack = self.proxy.send_command("OFFBOARD")
|
||
if ok:
|
||
# 等待模式切换
|
||
for _ in range(int(self.OFFBOARD_TIMEOUT / 0.1)):
|
||
st = self.proxy.get_latest_state()
|
||
if st and st["drone"]["mode"] == "OFFBOARD":
|
||
logger.info("[TAKEOFF] 已进入 OFFBOARD")
|
||
break
|
||
time.sleep(0.1)
|
||
else:
|
||
logger.warning("[TAKEOFF] OFFBOARD 切换超时")
|
||
else:
|
||
logger.error("[TAKEOFF] OFFBOARD 指令失败: %s", ack)
|
||
|
||
# ---- ③ 高度到位 + OFFBOARD 模式 → 跳转巡航 ----
|
||
if reached_alt and mode == "OFFBOARD":
|
||
logger.info("\n[起飞完成] 高度 %.1fm，进入航点巡航", alt_rel)
|
||
self.state = MissionState.NAVIGATING
|
||
self.current_wp_index = 0
|
||
self._set_current_target_from_wp(0)
|
||
return
|
||
|
||
# ---- 爬升超时保护 ----
|
||
elapsed = time.time() - self._state_start_time
|
||
if elapsed > self.TAKEOFF_TIMEOUT:
|
||
logger.warning("[TAKEOFF] 超时 (%.1fs)，强行进入巡航", elapsed)
|
||
self.state = MissionState.NAVIGATING
|
||
self.current_wp_index = 0
|
||
self._set_current_target_from_wp(0)
|
||
return
|
||
|
||
logger.info("[起飞中] alt=%.1f/%.1f mode=%s 耗时=%.1fs",
|
||
alt_rel, target_up, mode, elapsed)
|
||
|
||
except (KeyError, TypeError) as e:
|
||
logger.warning("[TAKEOFF] 异常: %s", e)
|
||
|
||
# ---- 航点导航 ----
|
||
|
||
def _handle_navigating(self, state):
|
||
"""
|
||
[NAVIGATING] 每帧发送 POSITION SETPOINT 前往当前航点
|
||
|
||
到达判定后推进索引；所有航点飞完后根据 return_to_home
|
||
决定返航或悬停。
|
||
"""
|
||
if not self.waypoints or self.current_wp_index >= len(self.waypoints):
|
||
self._finish_navigation()
|
||
return
|
||
|
||
wp = self.waypoints[self.current_wp_index]
|
||
cmd = wp.get("command", "WAYPOINT")
|
||
|
||
# 特殊航点分支选择
|
||
# 起飞航点
|
||
if cmd == "TAKEOFF":
|
||
# 飞到原点 + 目标高度
|
||
self.proxy.send_setpoint(
|
||
x=0, y=0, z=wp.get("z", self.target_altitude),
|
||
yaw=wp.get("yaw", 0.0),control_mode="POSITION",
|
||
)
|
||
current_z = self._get_current_z(state)
|
||
target_z = wp.get("z", self.target_altitude)
|
||
if abs(current_z - target_z) < 0.5:
|
||
self.current_wp_index += 1
|
||
return
|
||
|
||
# 降落航点
|
||
if cmd == "LAND":
|
||
logger.info("🛬 执行降落")
|
||
self.proxy.send_command("LAND")
|
||
self.state = MissionState.LANDING
|
||
return
|
||
|
||
|
||
wp_x = wp["x"]
|
||
wp_y = wp["y"]
|
||
wp_z = wp.get("z", self.target_altitude)
|
||
wp_yaw = wp.get("yaw", 0.0)
|
||
|
||
# ---- 发送 POSITION SETPOINT（通过自有 proxy） ----
|
||
# 此 SETPOINT 让 PX4 飞向目标航点
|
||
self.proxy.send_setpoint(
|
||
x=wp_x, y=wp_y, z=wp_z, yaw=wp_yaw,
|
||
control_mode="POSITION",
|
||
)
|
||
|
||
# ---- 共享目标 ----
|
||
self.current_target = {"x": wp_x, "y": wp_y, "z": wp_z, "yaw": wp_yaw}
|
||
|
||
# ---- 到达判定 ----
|
||
current_z = self._get_current_z(state)
|
||
dist = _dist_3d(self._est_x, self._est_y, current_z,
|
||
wp_x, wp_y, wp_z)
|
||
|
||
if dist < self.arrival_radius:
|
||
logger.info("\n✅ [到达航点 %d/%d] dist=%.2fm",
|
||
self.current_wp_index + 1, len(self.waypoints), dist)
|
||
self.state = MissionState.HOLD_TASK
|
||
self.hold_start_time = time.time()
|
||
else:
|
||
logger.info("[巡航中] 航点 [%d/%d] 距离=%.2fm",
|
||
self.current_wp_index + 1, len(self.waypoints), dist)
|
||
|
||
def _finish_navigation(self):
|
||
"""所有航点飞完后的跳转"""
|
||
if self.return_to_home:
|
||
logger.info("\n[巡航完成] 返回起始点...")
|
||
self.state = MissionState.RETURNING
|
||
self.current_target = {"x": 0, "y": 0,
|
||
"z": self.target_altitude, "yaw": 0}
|
||
else:
|
||
logger.info("\n[巡航完成] 末端悬停...")
|
||
self.state = MissionState.HOLD_FINAL
|
||
self._state_start_time = time.time()
|
||
|
||
# ---- 悬停搜索 ----
|
||
|
||
def _handle_hold_task(self, state, target_detected):
|
||
"""
|
||
[HOLD_TASK] 抵达航点后的悬停搜索
|
||
|
||
策略：
|
||
- 检测到目标 → VISUAL_TRACKING
|
||
- 超时无目标 → 下一个航点
|
||
- 持续发送 POSITION SETPOINT 悬停
|
||
"""
|
||
# ---- 发现目标 ----
|
||
if target_detected:
|
||
logger.info("\n🎯 [目标锁定] 切入视觉追踪状态！")
|
||
self.state = MissionState.VISUAL_TRACKING
|
||
self.tracking_start_time = time.time()
|
||
self._target_lost_start = None
|
||
self.tracking_completed = False
|
||
return
|
||
|
||
# ---- 保持当前位置悬停 ----
|
||
wp = self.waypoints[self.current_wp_index]
|
||
self.proxy.send_setpoint(
|
||
x=wp["x"], y=wp["y"],
|
||
z=wp.get("z", self.target_altitude),
|
||
yaw=wp.get("yaw", 0),
|
||
control_mode="POSITION",
|
||
)
|
||
self.current_target = {
|
||
"x": wp["x"], "y": wp["y"],
|
||
"z": wp.get("z", self.target_altitude),
|
||
"yaw": wp.get("yaw", 0),
|
||
}
|
||
|
||
# ---- 超时判定 ----
|
||
elapsed = time.time() - self.hold_start_time
|
||
if elapsed >= self.hold_duration:
|
||
logger.info("\n[未发现目标] 搜寻超时 (%.1fs)，前往下一航点",
|
||
self.hold_duration)
|
||
self.current_wp_index += 1
|
||
self.state = MissionState.NAVIGATING
|
||
else:
|
||
logger.info("[检索中] 航点 %d 剩余 %.1fs",
|
||
self.current_wp_index + 1,
|
||
self.hold_duration - elapsed)
|
||
|
||
# ---- 视觉追踪 ----
|
||
|
||
def _handle_visual_tracking(self, state, target_detected):
|
||
"""
|
||
[VISUAL_TRACKING] 🎯 完全让渡控制权给 UAVControlLoop
|
||
|
||
【核心设计】
|
||
MissionManager 在此状态下 **不发送任何 SETPOINT**。
|
||
UAVControlLoop (20Hz 独立线程) 通过其自有 proxy 发送
|
||
VELOCITY SETPOINT 进行视觉闭环追踪。
|
||
|
||
本模块仅负责追踪退出条件的判定和状态切换。
|
||
|
||
【退出条件】
|
||
1. self.tracking_completed == True（外部模块设置）
|
||
2. 目标丢失超过 target_lost_timeout 秒
|
||
3. 追踪总时长超过 tracking_max_duration（兜底）
|
||
"""
|
||
# ---- 不发送任何 SETPOINT — 控制权已让渡 ----
|
||
|
||
tracking_elapsed = time.time() - self.tracking_start_time
|
||
|
||
# ---- 兜底超时 ----
|
||
if tracking_elapsed > self.tracking_max_duration:
|
||
logger.info("\n⏰ [追踪超时] %.1fs ≥ %.1fs",
|
||
tracking_elapsed, self.tracking_max_duration)
|
||
self._exit_tracking()
|
||
return
|
||
|
||
# ---- 外部完成信号 ----
|
||
if self.tracking_completed:
|
||
logger.info("\n✅ [追踪完成] 外部模块报告完成，返回巡航")
|
||
self.tracking_completed = False
|
||
self._exit_tracking()
|
||
return
|
||
|
||
# ---- 目标丢失超时 ----
|
||
if not target_detected:
|
||
if self._target_lost_start is None:
|
||
self._target_lost_start = time.time()
|
||
else:
|
||
lost_elapsed = time.time() - self._target_lost_start
|
||
if lost_elapsed >= self.target_lost_timeout:
|
||
logger.info("\n⏰ [丢失超时] %.1fs，放弃追踪",
|
||
lost_elapsed)
|
||
self._exit_tracking()
|
||
return
|
||
logger.info("[追踪中] 丢失 %.1fs/%.1fs",
|
||
lost_elapsed, self.target_lost_timeout)
|
||
else:
|
||
self._target_lost_start = None
|
||
|
||
logger.info("[追踪中] 进行中... %.1fs", tracking_elapsed)
|
||
|
||
def _exit_tracking(self):
|
||
"""退出 VISUAL_TRACKING 的统一出口"""
|
||
self.current_wp_index += 1
|
||
self.state = MissionState.NAVIGATING
|
||
self._target_lost_start = None
|
||
logger.info("[状态切换] VISUAL_TRACKING → NAVIGATING，"
|
||
"前往航点 %d", self.current_wp_index + 1)
|
||
|
||
# ---- 返航 ----
|
||
|
||
def _handle_returning(self, state):
|
||
"""
|
||
[RETURNING] 发送 POSITION SETPOINT 到起始点 (0,0,z)
|
||
"""
|
||
self.proxy.send_setpoint(
|
||
x=0, y=0, z=self.target_altitude, yaw=0,
|
||
control_mode="POSITION",
|
||
)
|
||
self.current_target = {"x": 0, "y": 0,
|
||
"z": self.target_altitude, "yaw": 0}
|
||
|
||
current_z = self._get_current_z(state)
|
||
dist = _dist_3d(self._est_x, self._est_y, current_z,
|
||
0.0, 0.0, self.target_altitude)
|
||
logger.info("[返航中] 距离=%.2fm", dist)
|
||
|
||
if dist < self.arrival_radius:
|
||
logger.info("\n[到达] 已返回起始点，发送 LAND 指令...")
|
||
ok, ack = self.proxy.send_command("LAND")
|
||
if ok:
|
||
self.state = MissionState.LANDING
|
||
else:
|
||
logger.warning("[返航] LAND 失败: %s", ack)
|
||
|
||
# ---- 降落 ----
|
||
|
||
def _handle_landing(self, state):
|
||
"""
|
||
[LANDING] 监控 disarm 状态判断着陆
|
||
"""
|
||
try:
|
||
armed = state["drone"].get("armed", False)
|
||
alt_rel = state["drone"].get("alt_rel", 0.0)
|
||
logger.info("[降落中] 高度=%.1fm 已解锁=%s",
|
||
alt_rel, not armed)
|
||
|
||
if not armed:
|
||
logger.info("\n[完结] 已着陆上锁，任务结束")
|
||
self.state = MissionState.FINISHED
|
||
except (KeyError, TypeError) as e:
|
||
logger.warning("[LANDING] 异常: %s", e)
|
||
|
||
# ---- 末端悬停 ----
|
||
|
||
def _handle_hold_final(self, state):
|
||
"""
|
||
[HOLD_FINAL] 末端悬停，保持最终航点位置
|
||
"""
|
||
if self.waypoints:
|
||
final_wp = self.waypoints[-1]
|
||
else:
|
||
final_wp = {"x": 0, "y": 0,
|
||
"z": self.target_altitude, "yaw": 0}
|
||
|
||
self.proxy.send_setpoint(
|
||
x=final_wp["x"], y=final_wp["y"],
|
||
z=final_wp.get("z", self.target_altitude),
|
||
yaw=final_wp.get("yaw", 0),
|
||
control_mode="POSITION",
|
||
)
|
||
self.current_target = {
|
||
"x": final_wp["x"], "y": final_wp["y"],
|
||
"z": final_wp.get("z", self.target_altitude),
|
||
"yaw": final_wp.get("yaw", 0),
|
||
}
|
||
logger.info("[挂起] 保持 (%.1f, %.1f, %.1f) 等待接管",
|
||
final_wp["x"], final_wp["y"],
|
||
final_wp.get("z", self.target_altitude))
|
||
|
||
# ================================================================
|
||
# 辅助
|
||
# ================================================================
|
||
|
||
def _set_current_target_from_wp(self, index):
|
||
"""从航点列表更新 current_target"""
|
||
if self.waypoints and index < len(self.waypoints):
|
||
wp = self.waypoints[index]
|
||
self.current_target = {
|
||
"x": wp["x"], "y": wp["y"],
|
||
"z": wp.get("z", self.target_altitude),
|
||
"yaw": wp.get("yaw", 0),
|
||
}
|
||
|
||
|
||
def set_waypoints(self, waypoints):
|
||
"""由外部调用，动态注入/更新航点列表"""
|
||
self.waypoints = list(waypoints)
|
||
self.current_wp_index = 0
|
||
self._waypoints_ready = True # 这个标志位结合 WAITING_WAYPOINTS 状态使用
|
||
logger.info("MissionManager 航点已更新，共 %d 个", len(waypoints))
|
||
|
||
|
||
# ================================================================
|
||
# 🚨 紧急停靠（外部调用）
|
||
# ================================================================
|
||
|
||
def emergency_stop(self):
|
||
"""
|
||
外部触发的紧急降落入口（可在任何状态下调用）
|
||
|
||
行为：
|
||
1. 发送 LAND 指令到飞控
|
||
2. 将状态机切换到 LANDING 状态
|
||
3. 记录警告日志（供事后分析）
|
||
|
||
注意：
|
||
- 此方法可在任何线程中调用（内部使用 proxy，proxy 本身是线程安全的）
|
||
- 调用后，主循环会检测到 state == LANDING 并执行降落逻辑
|
||
- 如果已经在 LANDING 状态，重复调用只会重新发送 LAND 指令
|
||
"""
|
||
logger.warning("🚨 [emergency_stop] 紧急降落触发！")
|
||
|
||
# 1. 发送 LAND 指令（不管当前状态如何，直接发）
|
||
ok, ack = self.proxy.send_command("LAND")
|
||
if ok:
|
||
logger.info("✅ LAND 指令已发送")
|
||
else:
|
||
logger.error(f"❌ LAND 指令发送失败: {ack}")
|
||
|
||
# 2. 状态机强制切换到 LANDING（让主循环接管后续降落逻辑）
|
||
self.state = MissionState.LANDING
|
||
|
||
# 3. 重置一些计时器，让降落逻辑从当前状态开始
|
||
self._state_start_time = time.time()
|