基于ZMQ Router重构状态机相关代码以及readme和.sh文件的修改
This commit is contained in:
+664
-158
@@ -1,208 +1,714 @@
|
||||
"""
|
||||
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
|
||||
|
||||
logger = logging.getLogger(__name__)
|
||||
|
||||
|
||||
class MissionState(Enum):
|
||||
INIT = 0 # 初始化/等待连接
|
||||
ARMING = 1 # 解锁中
|
||||
TAKEOFF = 2 # 自动起飞中
|
||||
NAVIGATING = 3 # 航点巡航导航
|
||||
HOLD_TASK = 4 # 到达航点后的原地搜索/悬停
|
||||
VISUAL_TRACKING = 5 # 🎯 视觉追踪中（由视觉主循环接管控制权）
|
||||
RETURNING = 6 # 返航中
|
||||
LANDING = 7 # 自动降落中
|
||||
FINISHED = 8 # 任务结束/已上锁
|
||||
HOLD_FINAL = 9 # 结束巡点后原地悬停（当 return_to_home 为 False 时使用）
|
||||
"""无人机任务状态枚举"""
|
||||
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:
|
||||
def __init__(self, drone, waypoints, target_altitude=1.5, arrival_radius=0.3, hold_duration=5.0,
|
||||
return_to_home=True):
|
||||
"""
|
||||
🛸 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):
|
||||
"""
|
||||
:param drone: 实例化的 DroneController 底层驱动对象
|
||||
:param waypoints: 航点列表 [{'x': 1.0, 'y': 2.0, 'z': -1.5, 'yaw': 0}, ... ]
|
||||
:param target_altitude: 默认起飞高度（米，NED中向上为负值）
|
||||
:param arrival_radius: 航点到达判定半径（米）
|
||||
:param hold_duration: 抵达巡点后，没有发现目标时的最大搜寻/悬停时间（秒）
|
||||
:param return_to_home: 航点巡完后是否返航，False则在最后一个点原地悬停
|
||||
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.drone = drone
|
||||
self.waypoints = waypoints
|
||||
self.target_altitude = target_altitude if target_altitude < 0 else -target_altitude
|
||||
# ---- 依赖注入 ----
|
||||
self.proxy = proxy
|
||||
|
||||
# ---- 任务参数 ----
|
||||
# 航点列表（外部队列已经使用 NED 坐标系，Z 负值向上）
|
||||
self.waypoints = list(waypoints) if waypoints else []
|
||||
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.state = MissionState.INIT
|
||||
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()
|
||||
|
||||
# 起始点（Home）坐标缓存
|
||||
self.home_x = 0.0
|
||||
self.home_y = 0.0
|
||||
self.home_z = 0.0
|
||||
# ================================================================
|
||||
# 追踪完成信号（外部设置）
|
||||
# ================================================================
|
||||
# UAVControlLoop 或 run_mission.py 在判定视觉追踪完成时置 True
|
||||
self.tracking_completed = False
|
||||
# 追踪最大时长兜底
|
||||
self.tracking_max_duration = 30.0
|
||||
|
||||
def _is_tracking_task_complete(self):
|
||||
"""
|
||||
🛠️ 预留接口：判断视觉追踪任务是否完成
|
||||
你可以根据需求修改内部逻辑。例如：
|
||||
- 当前：假设持续追踪 5 秒视为完成任务。
|
||||
- 后续扩展：可以引入与目标的物理距离、传感器状态等。
|
||||
"""
|
||||
tracking_duration = time.time() - self.tracking_start_time
|
||||
target_hold_time = 5.0 # 默认设定5秒
|
||||
logger.info("MissionManager 初始化: %d 个航点, 目标高度=%.1fm, "
|
||||
"到达半径=%.1fm, 搜寻时长=%.1fs, 返航=%s",
|
||||
len(self.waypoints), -self.target_altitude,
|
||||
self.arrival_radius, self.hold_duration,
|
||||
self.return_to_home)
|
||||
|
||||
if tracking_duration >= target_hold_time:
|
||||
return True
|
||||
return False
|
||||
# ================================================================
|
||||
# 主入口
|
||||
# ================================================================
|
||||
|
||||
def update(self, target_detected=False):
|
||||
"""
|
||||
状态机高频核心业务逻辑，由外部主循环驱动
|
||||
:param target_detected: 当前帧是否发现了视觉追踪目标
|
||||
状态机高频核心业务逻辑，由外部主循环驱动（建议 ≥20Hz）
|
||||
|
||||
Args:
|
||||
target_detected: 当前帧视觉检测是否有目标
|
||||
|
||||
Raises:
|
||||
FailsafeTriggered: 检测到非法模式切换时抛出
|
||||
"""
|
||||
# 0. 越权检查（最强人肉兜底）：只要切出自主控制模式，立刻暴力熔断
|
||||
if self.state not in [MissionState.INIT, MissionState.FINISHED]:
|
||||
if self.drone.current_mode not in ['GUIDED', 'OFFBOARD']:
|
||||
print(f"\n⚠️ [Failsafe] 检测到飞控模式被切为: {self.drone.current_mode}! 外部人工强行接管，熔断任务！")
|
||||
self.state = MissionState.FINISHED
|
||||
raise FailsafeTriggered("安全员强行接管")
|
||||
# ---------------------------------------------------------------
|
||||
# 步骤 0：获取最新 STATE + 新鲜度校验 + Failsafe
|
||||
# ---------------------------------------------------------------
|
||||
state = self.proxy.get_latest_state()
|
||||
|
||||
# 1. INIT: 等待飞控连接并获取当前位置作为 Home 点
|
||||
if self.state == MissionState.INIT:
|
||||
if self.drone.is_connected:
|
||||
print("[INIT] 飞控已连上。获取当前坐标为起始点...")
|
||||
self.home_x, self.home_y, self.home_z = self.drone.local_position
|
||||
print(f"[INIT] 起始点坐标: ({self.home_x:.2f}, {self.home_y:.2f}, {self.home_z:.2f})")
|
||||
print("[INIT] 正在下发 GUIDED/OFFBOARD 模式并尝试解锁...")
|
||||
# ---- 新鲜度校验（协议 §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
|
||||
|
||||
target_mode = "OFFBOARD" if "OFFBOARD" in self.drone.master.mode_mapping() else "GUIDED"
|
||||
self.drone.set_mode(target_mode)
|
||||
self.drone.arm()
|
||||
self.state = MissionState.ARMING
|
||||
# ---- 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}"
|
||||
)
|
||||
|
||||
# 2. ARMING: 监测解锁状态
|
||||
elif self.state == MissionState.ARMING:
|
||||
if self.drone.is_armed and self.drone.current_mode in ['GUIDED', 'OFFBOARD']:
|
||||
print("\n[解锁成功] 正在下发自动起飞指令...")
|
||||
# 下发位置到期望高度
|
||||
self.drone.send_target_position(self.home_x, self.home_y, self.target_altitude, 0)
|
||||
self.state = MissionState.TAKEOFF
|
||||
# ---- 无 STATE 时仅 INIT 可容忍 ----
|
||||
if state is None:
|
||||
if self.state == MissionState.INIT:
|
||||
logger.info("[INIT] 等待 Router 状态推送...")
|
||||
return
|
||||
|
||||
# 3. TAKEOFF: 自动起飞爬升判定
|
||||
elif self.state == MissionState.TAKEOFF:
|
||||
current_z = self.drone.local_position[2]
|
||||
# 持续发送起飞点位置，激活控制链路
|
||||
self.drone.send_target_position(self.home_x, self.home_y, self.target_altitude, 0)
|
||||
# ---------------------------------------------------------------
|
||||
# 步骤 1：更新本地位置估计
|
||||
# ---------------------------------------------------------------
|
||||
self._update_local_position(state)
|
||||
|
||||
# NED 坐标系下 Z 为负数，判断是否接近目标高度
|
||||
if abs(current_z - self.target_altitude) < self.arrival_radius:
|
||||
print("\n[起飞完成] 达到预定高度。开始前往第 1 个巡航航点...")
|
||||
# ---------------------------------------------------------------
|
||||
# 步骤 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 序列
|
||||
"""
|
||||
try:
|
||||
drone = state["drone"]
|
||||
armed = drone.get("armed", False)
|
||||
mode = drone.get("mode", "")
|
||||
alt_rel = drone.get("alt_rel", 0.0)
|
||||
|
||||
logger.info("[INIT] mode=%s armed=%s alt_rel=%.1fm",
|
||||
mode, armed, alt_rel)
|
||||
|
||||
# A) 已在 OFFBOARD + 已解锁 → 直接巡航
|
||||
if armed and mode == "OFFBOARD":
|
||||
logger.info("[INIT] 已在 OFFBOARD，直接进入航点巡航")
|
||||
self.state = MissionState.NAVIGATING
|
||||
self.current_wp_index = 0
|
||||
|
||||
# 4. NAVIGATING: 航点导航巡航
|
||||
elif self.state == MissionState.NAVIGATING:
|
||||
if not self.waypoints or self.current_wp_index >= len(self.waypoints):
|
||||
# 航点全部飞完
|
||||
if self.return_to_home:
|
||||
print("\n[巡航完成] 所有航点已巡视完毕。正在返回起始点上方...")
|
||||
self.state = MissionState.RETURNING
|
||||
else:
|
||||
print("\n[巡航完成] 所有航点已巡视完毕。根据配置切换至末端就地悬停...")
|
||||
self.state = MissionState.HOLD_FINAL
|
||||
self._set_current_target_from_wp(0)
|
||||
return
|
||||
|
||||
# 获取当前目标航点
|
||||
wp = self.waypoints[self.current_wp_index]
|
||||
self.drone.send_target_position(wp['x'], wp['y'], wp['z'], wp['yaw'])
|
||||
|
||||
# 计算水平及高度距离
|
||||
dx = wp['x'] - self.drone.local_position[0]
|
||||
dy = wp['y'] - self.drone.local_position[1]
|
||||
dz = wp['z'] - self.drone.local_position[2]
|
||||
dist_xyz = math.sqrt(dx ** 2 + dy ** 2 + dz ** 2)
|
||||
|
||||
print(
|
||||
f"\n\r[巡航中] 前往航点 [{self.current_wp_index + 1}/{len(self.waypoints)}] | 距离目标: {dist_xyz:.2f}m",
|
||||
end="", flush=True)
|
||||
|
||||
if dist_xyz < self.arrival_radius:
|
||||
print(f"\n[到达航点 {self.current_wp_index + 1}] 进入原地悬停与视觉检索阶段...")
|
||||
self.state = MissionState.HOLD_TASK
|
||||
self.hold_start_time = time.time()
|
||||
|
||||
# 5. HOLD_TASK: 抵达巡点后的搜索悬停阶段
|
||||
elif self.state == MissionState.HOLD_TASK:
|
||||
# 优先判定视觉：如果在巡点悬停搜寻期间，视觉发现了目标
|
||||
if target_detected:
|
||||
print("\n🎯 [目标锁定] 视觉检测到目标！立刻中断悬停，切入 [视觉追踪] 状态！")
|
||||
self.state = MissionState.VISUAL_TRACKING
|
||||
self.tracking_start_time = time.time()
|
||||
# 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
|
||||
|
||||
# 如果没有发现目标，则保持航点位置，并检查悬停是否超时
|
||||
wp = self.waypoints[self.current_wp_index]
|
||||
self.drone.send_target_position(wp['x'], wp['y'], wp['z'], wp['yaw'])
|
||||
# C) 已解锁但非 OFFBOARD
|
||||
if armed and mode != "OFFBOARD":
|
||||
logger.info("[INIT] 已解锁但 mode=%s，进入起飞序列", mode)
|
||||
self.state = MissionState.ARMING
|
||||
self._state_start_time = time.time()
|
||||
|
||||
elapsed_time = time.time() - self.hold_start_time
|
||||
print(f"\r[检索中] 保持航点中... 剩余搜寻时间: {max(0.0, self.hold_duration - elapsed_time):.1f}s", end="",
|
||||
flush=True)
|
||||
except (KeyError, TypeError) as e:
|
||||
logger.warning("[INIT] 解析异常: %s", e)
|
||||
|
||||
if elapsed_time >= self.hold_duration:
|
||||
print(f"\n[未发现目标] 搜寻超时。放弃当前点，前往下一个航点...")
|
||||
self.current_wp_index += 1
|
||||
self.state = MissionState.NAVIGATING
|
||||
# ---- 解锁 ----
|
||||
|
||||
# 6. VISUAL_TRACKING: 🎯 视觉闭环追踪状态（核心新增）
|
||||
elif self.state == MissionState.VISUAL_TRACKING:
|
||||
# 在此状态下，高层状态机【不主动发送】绝对位置坐标，把控制权完全让渡给主循环的 PID 速度控制
|
||||
# 我们只需要在这里进行任务结束判定
|
||||
print(f"\r[追踪中] 正在执行AI视觉闭环精准追踪... 计时: {time.time() - self.tracking_start_time:.1f}s",
|
||||
end="", flush=True)
|
||||
def _handle_arming(self, state):
|
||||
"""
|
||||
[ARMING] 等待解锁确认，成功后进入 TAKEOFF
|
||||
"""
|
||||
try:
|
||||
armed = state["drone"].get("armed", False)
|
||||
|
||||
if self._is_tracking_task_complete():
|
||||
print("\n✅ [任务完成] 当前目标追踪任务圆满完成！重置追踪器，继续执行航点巡航...")
|
||||
self.current_wp_index += 1 # 切换到下一个航点
|
||||
self.state = MissionState.NAVIGATING
|
||||
|
||||
# 7. RETURNING: 自动返航至起始点上方
|
||||
elif self.state == MissionState.RETURNING:
|
||||
self.drone.send_target_position(self.home_x, self.home_y, self.target_altitude, 0)
|
||||
|
||||
dist_to_home = math.sqrt((self.home_x - self.drone.local_position[0]) ** 2 +
|
||||
(self.home_y - self.drone.local_position[1]) ** 2)
|
||||
print(f"\r[返航中] 正在飞回起始点位置 | 剩余水平距离: {dist_to_home:.2f}m", end="", flush=True)
|
||||
|
||||
if dist_to_home < self.arrival_radius:
|
||||
print("\n[到达] 已安全返回起始点上方。正在下发自动降落指令 (LAND)...")
|
||||
if self.drone.set_mode('LAND'):
|
||||
self.state = MissionState.LANDING
|
||||
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:
|
||||
print("⚠️ 降落模式切换失败，正在重新下发...")
|
||||
logger.info("[ARMING] 等待解锁... (%.1fs)", elapsed)
|
||||
return
|
||||
|
||||
# 8. LANDING: 自动降落中
|
||||
elif self.state == MissionState.LANDING:
|
||||
print(f"\r[降落中] 当前高度: {self.drone.local_position[2]:+.2f}m | 解锁状态: {self.drone.is_armed}",
|
||||
end="", flush=True)
|
||||
if not self.drone.is_armed:
|
||||
print("\n[完结] 无人机已安全着陆并自动上锁。整个任务流结束。")
|
||||
# ---- 解锁成功 → 起飞 ----
|
||||
takeoff_alt = -self.target_altitude # 正值（协议 altitude 为正）
|
||||
logger.info("\n[解锁成功] 发送起飞指令 (alt=%.1fm)", takeoff_alt)
|
||||
|
||||
ok, ack = self.proxy.send_waypoint(
|
||||
action="TAKEOFF", alt=takeoff_alt,
|
||||
alt_frame="RELATIVE", speed=2.0,
|
||||
)
|
||||
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]
|
||||
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)
|
||||
|
||||
# 9. HOLD_FINAL: 末端就地悬停
|
||||
elif self.state == MissionState.HOLD_FINAL:
|
||||
final_wp = self.waypoints[-1] if self.waypoints else {'x': self.home_x, 'y': self.home_y,
|
||||
'z': self.target_altitude, 'yaw': 0}
|
||||
self.drone.send_target_position(final_wp['x'], final_wp['y'], final_wp['z'], final_wp['yaw'])
|
||||
print(
|
||||
f"\r[挂起] 无人机保持在最终航点悬停。坐标:({final_wp['x']}, {final_wp['y']}, {final_wp['z']}) | 等待人工接管中...",
|
||||
end="", flush=True)
|
||||
# ---- 末端悬停 ----
|
||||
|
||||
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),
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user