wenchunandClaude Opus 4.7
415aed088a
1. mavlinkROS2Nodes: position_gnss topic 改發 relative_altitude 而非 AMSL， 讓 drone_gps['alt'] 在實飛與 SITL 都代表「相對 home 高度」 2. mission_planner: Circle stage1.z 改用 final_z（不再用 current_z 做中點插值）， 避免實飛時 current_z 是 AMSL 而把過渡高度衝到海拔值的一半 3. mission_executor: 到達判定改為 hover-stable（radius * 1.5 hysteresis + 穩定計時），預設 arrival_radius 2→4m，解決實飛 GPS 抖動永遠進不了 2m radius 的卡死問題 4. mission_executor: 新增 progress log，每 3 秒印一行距離與穩定度， 方便實飛 debug 卡點問題 5. mission_group + gui: 暴露 arrival_radius / hover_stable_sec 給 GUI 任務參數面板， 每次 start 重讀以支援中途調整 Co-Authored-By: Claude Opus 4.7 <noreply@anthropic.com>
432 lines
17 KiB
Python
432 lines
17 KiB
Python
#!/usr/bin/env python3
|
||
"""
|
||
任務執行模組
|
||
管理多架無人機的 GUIDED 模式飛行控制迴圈
|
||
|
||
設計:
|
||
- 每架無人機持有一個航點序列，逐點推進
|
||
- 各自到達就各自切換到下一個航點
|
||
- 事件驅動發送：航點切換時送一次，收到 send_result 才決定重送/前進
|
||
- 失敗策略 (b)：單機重試 MAX_RETRY 次仍失敗 → 該機 fallback LOITER，其他機繼續
|
||
- Rendezvous barrier：在指定 wp index 等所有活著的機到齊才一起推進，有 timeout 保護
|
||
- 用 QTimer 驅動到達判定，在 Qt 主線程執行
|
||
- 暫停 = 停止到達判定 + 停止新指令
|
||
"""
|
||
import asyncio
|
||
import math
|
||
import time
|
||
from enum import Enum
|
||
from PyQt6.QtCore import QObject, QTimer, pyqtSignal
|
||
|
||
|
||
def _log(level, message):
|
||
print(f"[{level}] {message}")
|
||
|
||
|
||
class MissionState(Enum):
|
||
"""整體任務狀態"""
|
||
IDLE = "idle"
|
||
RUNNING = "running"
|
||
PAUSED = "paused"
|
||
|
||
|
||
class TaskStatus(Enum):
|
||
"""單機任務狀態"""
|
||
NORMAL = "normal"
|
||
RETRYING = "retrying"
|
||
WAITING_AT_BARRIER = "waiting_at_barrier"
|
||
FALLBACK_LOITER = "fallback_loiter"
|
||
|
||
|
||
class DroneTask:
|
||
"""單架無人機的任務資料"""
|
||
__slots__ = (
|
||
'drone_id', 'sysid', 'waypoints', 'wp_index', 'done',
|
||
'sent_current_wp', 'fail_count', 'status', 'waiting_since',
|
||
'entered_radius_at', 'last_log_at',
|
||
)
|
||
|
||
def __init__(self, drone_id, sysid, waypoints):
|
||
self.drone_id = drone_id
|
||
self.sysid = sysid
|
||
self.waypoints = waypoints
|
||
self.wp_index = 0
|
||
self.done = len(waypoints) == 0
|
||
self.sent_current_wp = False
|
||
self.fail_count = 0
|
||
self.status = TaskStatus.NORMAL
|
||
self.waiting_since = 0.0 # monotonic time 進入 WAITING_AT_BARRIER 的瞬間
|
||
self.entered_radius_at = 0.0 # 進入 arrival_radius 的時間，0 = 尚未進入
|
||
self.last_log_at = 0.0 # 上次印 distance log 的時間
|
||
|
||
@property
|
||
def current_target(self):
|
||
if self.done or self.wp_index >= len(self.waypoints):
|
||
return None
|
||
return self.waypoints[self.wp_index]
|
||
|
||
@property
|
||
def total_waypoints(self):
|
||
return len(self.waypoints)
|
||
|
||
|
||
class MissionExecutor(QObject):
|
||
"""
|
||
任務執行器 (事件驅動版 + rendezvous barrier)
|
||
|
||
planned_waypoints 格式:
|
||
{
|
||
'drone_ids': ['s0_1', 's0_2', ...],
|
||
'waypoints': [
|
||
[(lat,lon,alt), ...], # drone 0
|
||
[(lat,lon,alt), ...], # drone 1
|
||
],
|
||
'rendezvous_indices': [3, 7, ...], # 選填；缺省 = 全程不同步（各自跑）
|
||
}
|
||
"""
|
||
|
||
MAX_RETRY = 3
|
||
|
||
drone_waypoint_reached = pyqtSignal(str, int, int) # (drone_id, wp_index, total)
|
||
task_status_changed = pyqtSignal(str, str, str) # (drone_id, status, message)
|
||
mission_completed = pyqtSignal()
|
||
|
||
def __init__(self, sender, drone_gps, monitor=None,
|
||
arrival_radius=4.0, tick_rate_hz=2.0,
|
||
barrier_timeout_sec=20.0,
|
||
hover_stable_sec=2.0,
|
||
progress_log_interval_sec=3.0):
|
||
super().__init__()
|
||
self.sender = sender
|
||
self.drone_gps = drone_gps
|
||
self.monitor = monitor # 用於失敗 fallback 到 LOITER
|
||
self.arrival_radius = arrival_radius
|
||
self.barrier_timeout_sec = barrier_timeout_sec
|
||
# hover-stable 判定：進入 radius 後須穩定停留 hover_stable_sec 秒才算到達，
|
||
# 容忍 GPS 抖動跨越邊界（用 radius * 1.5 作 hysteresis）
|
||
self.hover_stable_sec = hover_stable_sec
|
||
self.hysteresis_factor = 1.5
|
||
self.progress_log_interval_sec = progress_log_interval_sec
|
||
self.state = MissionState.IDLE
|
||
self.tasks = {}
|
||
self.rendezvous_indices = set()
|
||
|
||
self._interval_ms = int(1000 / tick_rate_hz)
|
||
self._timer = QTimer()
|
||
self._timer.timeout.connect(self._tick)
|
||
|
||
# 只有 Ros2CommandSender 有 send_result signal；MavlinkSender 沒有
|
||
if hasattr(sender, 'send_result'):
|
||
sender.send_result.connect(self._on_send_result)
|
||
|
||
# ------------------------------------------------------------------ 公開方法
|
||
|
||
def start(self, planned_waypoints):
|
||
if self.state == MissionState.RUNNING:
|
||
_log("WARN", "任務已在執行中")
|
||
return
|
||
|
||
self.tasks.clear()
|
||
|
||
drone_ids = planned_waypoints['drone_ids']
|
||
waypoints_list = planned_waypoints['waypoints']
|
||
self.rendezvous_indices = set(
|
||
planned_waypoints.get('rendezvous_indices', []) or []
|
||
)
|
||
|
||
for i, drone_id in enumerate(drone_ids):
|
||
sysid = int(drone_id.split('_')[1])
|
||
self.tasks[drone_id] = DroneTask(drone_id, sysid, waypoints_list[i])
|
||
|
||
self.state = MissionState.RUNNING
|
||
self._timer.start(self._interval_ms)
|
||
|
||
total_wps = sum(t.total_waypoints for t in self.tasks.values())
|
||
rv_info = (
|
||
f"rendezvous WP={sorted(self.rendezvous_indices)}"
|
||
if self.rendezvous_indices else "無 rendezvous (各自跑)"
|
||
)
|
||
_log(
|
||
"INFO",
|
||
f"任務已啟動: {len(self.tasks)} 架無人機, "
|
||
f"共 {total_wps} 個航點, "
|
||
f"到達半徑={self.arrival_radius}m (hover-stable {self.hover_stable_sec}s), "
|
||
f"tick 週期={self._interval_ms}ms, "
|
||
f"barrier timeout={self.barrier_timeout_sec}s, "
|
||
f"{rv_info}",
|
||
)
|
||
|
||
def pause(self):
|
||
if self.state == MissionState.RUNNING:
|
||
self._timer.stop()
|
||
self.state = MissionState.PAUSED
|
||
_log("INFO", "任務已暫停")
|
||
|
||
def resume(self):
|
||
if self.state == MissionState.PAUSED:
|
||
self._timer.start(self._interval_ms)
|
||
self.state = MissionState.RUNNING
|
||
_log("INFO", "任務已恢復")
|
||
|
||
def stop(self):
|
||
self._timer.stop()
|
||
self.tasks.clear()
|
||
self.rendezvous_indices = set()
|
||
self.state = MissionState.IDLE
|
||
_log("INFO", "任務已停止")
|
||
|
||
# ------------------------------------------------------------------ 控制迴圈
|
||
|
||
def _tick(self):
|
||
"""
|
||
控制 tick：
|
||
Phase 1: 每台機做到達判定；非 barrier 點直接推進，barrier 點改成 WAITING
|
||
Phase 2: 檢查 barrier 能不能釋放（全員到齊或 timeout）
|
||
Phase 3: 每台未送過當前航點的機送一次
|
||
Phase 4: 全部完成檢查
|
||
"""
|
||
now = time.monotonic()
|
||
|
||
# ---- Phase 1: 到達判定（hover-stable） ----
|
||
for task in self.tasks.values():
|
||
if task.done or task.status == TaskStatus.FALLBACK_LOITER:
|
||
continue
|
||
# 已在 barrier 等待的不再跑到達判定
|
||
if task.status == TaskStatus.WAITING_AT_BARRIER:
|
||
continue
|
||
|
||
target = task.current_target
|
||
if target is None:
|
||
continue
|
||
|
||
gps = self.drone_gps.get(task.drone_id)
|
||
if gps is None:
|
||
continue
|
||
|
||
tgt_lat, tgt_lon, _ = target
|
||
distance = _haversine(gps['lat'], gps['lon'], tgt_lat, tgt_lon)
|
||
|
||
# hover-stable 判定：
|
||
# - 首次進入 radius → 記錄時間
|
||
# - 在 radius * hysteresis 內持續 → 保留計時
|
||
# - 離開 hysteresis → 重置
|
||
# - 在 radius 內穩定 hover_stable_sec 秒 → 視為到達
|
||
hysteresis_radius = self.arrival_radius * self.hysteresis_factor
|
||
if distance < self.arrival_radius:
|
||
if task.entered_radius_at == 0.0:
|
||
task.entered_radius_at = now
|
||
stable_for = now - task.entered_radius_at
|
||
if stable_for < self.hover_stable_sec:
|
||
self._maybe_log_progress(task, now, distance, stable_for)
|
||
continue
|
||
elif distance < hysteresis_radius and task.entered_radius_at > 0.0:
|
||
# 還在 hysteresis 內，計時不重置（容忍 GPS 抖動跨越邊界）
|
||
self._maybe_log_progress(task, now, distance, now - task.entered_radius_at)
|
||
continue
|
||
else:
|
||
# 離開 hysteresis：重置計時，繼續等待到達
|
||
task.entered_radius_at = 0.0
|
||
self._maybe_log_progress(task, now, distance, 0.0)
|
||
continue
|
||
|
||
# 到達當前 wp_index
|
||
task.entered_radius_at = 0.0 # 重置給下個 wp 用
|
||
if (task.wp_index in self.rendezvous_indices
|
||
and task.wp_index < task.total_waypoints - 1):
|
||
# rendezvous 點 → 不推進，進入 barrier 等待
|
||
task.status = TaskStatus.WAITING_AT_BARRIER
|
||
task.waiting_since = now
|
||
_log("INFO", f"{task.drone_id} 抵達 barrier WP {task.wp_index}，等待同伴")
|
||
self.task_status_changed.emit(
|
||
task.drone_id, task.status.value,
|
||
f"waiting at WP {task.wp_index}"
|
||
)
|
||
continue
|
||
|
||
# 一般點 → 直接推進
|
||
self._advance_waypoint(task, distance)
|
||
|
||
# ---- Phase 2: barrier 釋放檢查 ----
|
||
self._check_barriers(now)
|
||
|
||
# ---- Phase 3: 發送未送過的目標 ----
|
||
for task in self.tasks.values():
|
||
if task.done or task.status in (
|
||
TaskStatus.FALLBACK_LOITER, TaskStatus.WAITING_AT_BARRIER
|
||
):
|
||
continue
|
||
if task.sent_current_wp:
|
||
continue
|
||
target = task.current_target
|
||
if target is None:
|
||
continue
|
||
tgt_lat, tgt_lon, tgt_alt = target
|
||
task.sent_current_wp = True
|
||
self.sender.send_position_global(
|
||
task.drone_id, task.sysid, tgt_lat, tgt_lon, tgt_alt
|
||
)
|
||
|
||
# ---- Phase 4: 完成檢查 ----
|
||
all_done = all(
|
||
t.done or t.status == TaskStatus.FALLBACK_LOITER
|
||
for t in self.tasks.values()
|
||
)
|
||
if all_done and self.tasks:
|
||
self._timer.stop()
|
||
self.state = MissionState.IDLE
|
||
self.mission_completed.emit()
|
||
_log("INFO", "任務全部完成")
|
||
|
||
def _maybe_log_progress(self, task, now, distance, stable_for):
|
||
"""週期性印出 drone 與當前 wp 的距離，方便實飛 debug 卡點問題。"""
|
||
if now - task.last_log_at < self.progress_log_interval_sec:
|
||
return
|
||
task.last_log_at = now
|
||
if stable_for > 0:
|
||
stable_info = f", 在內側 {stable_for:.1f}s/{self.hover_stable_sec:.1f}s"
|
||
else:
|
||
stable_info = ""
|
||
_log(
|
||
"INFO",
|
||
f"{task.drone_id} → WP {task.wp_index}: "
|
||
f"距離 {distance:.1f}m (半徑 {self.arrival_radius:.1f}m{stable_info})",
|
||
)
|
||
|
||
def _advance_waypoint(self, task, arrived_distance):
|
||
"""把 task 推進一個航點，重置發送旗標。不處理 barrier 邏輯。"""
|
||
task.wp_index += 1
|
||
task.sent_current_wp = False
|
||
task.fail_count = 0
|
||
task.entered_radius_at = 0.0
|
||
task.last_log_at = 0.0
|
||
if task.wp_index >= task.total_waypoints:
|
||
task.done = True
|
||
self.drone_waypoint_reached.emit(
|
||
task.drone_id, task.wp_index, task.total_waypoints
|
||
)
|
||
_log(
|
||
"INFO",
|
||
f"{task.drone_id} 已完成所有航點 "
|
||
f"({task.total_waypoints}/{task.total_waypoints})",
|
||
)
|
||
return
|
||
self.drone_waypoint_reached.emit(
|
||
task.drone_id, task.wp_index, task.total_waypoints
|
||
)
|
||
_log(
|
||
"INFO",
|
||
f"{task.drone_id} 前往 WP {task.wp_index}/{task.total_waypoints} "
|
||
f"(到達距離: {arrived_distance:.1f}m)",
|
||
)
|
||
|
||
def _check_barriers(self, now):
|
||
"""檢查每個有人在等的 barrier 能不能釋放。"""
|
||
# 蒐集目前有哪些 barrier 有人卡著
|
||
waiting_groups = {} # barrier_idx -> [task, ...]
|
||
for task in self.tasks.values():
|
||
if task.status == TaskStatus.WAITING_AT_BARRIER:
|
||
waiting_groups.setdefault(task.wp_index, []).append(task)
|
||
|
||
if not waiting_groups:
|
||
return
|
||
|
||
for barrier_idx, waiting_tasks in waiting_groups.items():
|
||
# 判斷是否「全員到齊」：所有仍活著的機都要麼在此 barrier 等，要麼早已越過此 idx
|
||
everyone_arrived = True
|
||
for task in self.tasks.values():
|
||
if task.done or task.status == TaskStatus.FALLBACK_LOITER:
|
||
continue # 失敗機/完成機不擋
|
||
if (task.status == TaskStatus.WAITING_AT_BARRIER
|
||
and task.wp_index == barrier_idx):
|
||
continue # 在這裡等
|
||
if task.wp_index > barrier_idx:
|
||
continue # 早已越過（barrier 先前已對他釋放）
|
||
# 還在路上
|
||
everyone_arrived = False
|
||
break
|
||
|
||
force_reason = None
|
||
if not everyone_arrived:
|
||
# 檢查 timeout：以該 barrier 上最早進入等待的時間為準
|
||
oldest_wait = min(t.waiting_since for t in waiting_tasks)
|
||
if now - oldest_wait >= self.barrier_timeout_sec:
|
||
force_reason = f"timeout {self.barrier_timeout_sec:.0f}s"
|
||
_log("WARN", f"barrier WP {barrier_idx} {force_reason}，強制放行")
|
||
else:
|
||
continue # 還沒到齊、也還沒 timeout → 繼續等
|
||
|
||
# 釋放：把所有在此 barrier 等待的機一起推進
|
||
tag = "全員到齊" if force_reason is None else force_reason
|
||
_log("INFO", f"barrier WP {barrier_idx} 已釋放 ({tag})，推進 {len(waiting_tasks)} 架")
|
||
for task in waiting_tasks:
|
||
task.status = TaskStatus.NORMAL
|
||
msg = f"barrier WP {barrier_idx} released ({tag})"
|
||
self.task_status_changed.emit(
|
||
task.drone_id, task.status.value, msg
|
||
)
|
||
self._advance_waypoint(task, self.arrival_radius)
|
||
|
||
# ------------------------------------------------------------------ 結果回呼
|
||
|
||
def _on_send_result(self, drone_id, sysid, success, message):
|
||
"""Ros2CommandSender.send_result 的 slot"""
|
||
task = self.tasks.get(drone_id)
|
||
if task is None or task.done:
|
||
return
|
||
# 若已在 barrier 等待，舊指令的遲到回應不要再觸發重試/fallback
|
||
if task.status == TaskStatus.WAITING_AT_BARRIER:
|
||
return
|
||
|
||
if success:
|
||
if task.fail_count > 0 or task.status == TaskStatus.RETRYING:
|
||
task.status = TaskStatus.NORMAL
|
||
self.task_status_changed.emit(drone_id, task.status.value, "recovered")
|
||
task.fail_count = 0
|
||
return
|
||
|
||
task.fail_count += 1
|
||
_log("WARN", f"{drone_id} 發送失敗 {task.fail_count}/{self.MAX_RETRY}: {message}")
|
||
|
||
if task.fail_count < self.MAX_RETRY:
|
||
task.status = TaskStatus.RETRYING
|
||
task.sent_current_wp = False # 下個 tick 會重送
|
||
self.task_status_changed.emit(
|
||
drone_id, task.status.value,
|
||
f"retry {task.fail_count}/{self.MAX_RETRY}: {message}"
|
||
)
|
||
else:
|
||
task.status = TaskStatus.FALLBACK_LOITER
|
||
self.task_status_changed.emit(
|
||
drone_id, task.status.value,
|
||
f"fallback LOITER after {self.MAX_RETRY} fails: {message}"
|
||
)
|
||
_log("ERROR", f"{drone_id} 連續失敗 {self.MAX_RETRY} 次，切換至 LOITER")
|
||
self._fallback_to_loiter(drone_id)
|
||
|
||
def _fallback_to_loiter(self, drone_id):
|
||
"""用 monitor.set_mode 切 LOITER。set_mode 是 coroutine，透過 event loop 派送。"""
|
||
if self.monitor is None:
|
||
_log("WARN", f"無 monitor，無法將 {drone_id} 切換至 LOITER")
|
||
return
|
||
try:
|
||
loop = asyncio.get_event_loop()
|
||
except RuntimeError:
|
||
loop = asyncio.new_event_loop()
|
||
asyncio.set_event_loop(loop)
|
||
loop.create_task(self.monitor.set_mode(drone_id, "LOITER"))
|
||
|
||
|
||
# ------------------------------------------------------------------ 工具函式
|
||
|
||
def _haversine(lat1, lon1, lat2, lon2):
|
||
"""計算兩個 GPS 座標間的水平距離 (公尺)"""
|
||
R = 6371000.0
|
||
lat1_r = math.radians(lat1)
|
||
lat2_r = math.radians(lat2)
|
||
dlat = math.radians(lat2 - lat1)
|
||
dlon = math.radians(lon2 - lon1)
|
||
|
||
a = (math.sin(dlat / 2) ** 2 +
|
||
math.cos(lat1_r) * math.cos(lat2_r) * math.sin(dlon / 2) ** 2)
|
||
return R * 2 * math.atan2(math.sqrt(a), math.sqrt(1 - a))
|