diff --git a/src/GUI/command_sender.py b/src/GUI/command_sender.py index 329a0f0..a0ddd12 100644 --- a/src/GUI/command_sender.py +++ b/src/GUI/command_sender.py @@ -3,10 +3,16 @@ 指令發送模組 負責將目標位置轉成具體的通訊指令發送到飛控 -現階段: MavlinkSender (直接 pymavlink 發送) -未來: 替換為 ROS2Sender (發到 ROS2 topic，由 fc_network_adapter 轉發) +目前使用 Ros2CommandSender，經 fc_network_adapter 的 +/fc_network/vehicle/pos_global_int service 發送。 + +MavlinkSender 保留作為直連 SITL 的 fallback 工具，但已不是預設路徑。 """ +import asyncio +import traceback from abc import ABC, abstractmethod + +from PyQt6.QtCore import QObject, pyqtSignal from pymavlink import mavutil @@ -14,11 +20,12 @@ class CommandSender(ABC): """指令發送抽象介面""" @abstractmethod - def send_position_global(self, sysid, lat, lon, alt): + def send_position_global(self, drone_id, sysid, lat, lon, alt): """ 發送全球座標位置指令 Args: + drone_id: GUI 用的 drone 識別字串 (如 's0_11') sysid: 目標無人機的 MAVLink system ID lat: 緯度 (度) lon: 經度 (度) @@ -58,16 +65,8 @@ class MavlinkSender(CommandSender): self.mav = mavutil.mavlink_connection(connection_string) print(f"MavlinkSender 已建立連線: {connection_string}") - def send_position_global(self, sysid, lat, lon, alt): - """ - 發送 SET_POSITION_TARGET_GLOBAL_INT - - 注意: - - coordinate_frame 使用 MAV_FRAME_GLOBAL_RELATIVE_ALT_INT - 高度是相對起飛點的高度 (公尺) - - 如果 FormationPlanner 產出的 alt 是 AMSL 絕對高度， - 需要在外部先減去起飛點高度再傳入 - """ + def send_position_global(self, drone_id, sysid, lat, lon, alt): + """發送 SET_POSITION_TARGET_GLOBAL_INT。drone_id 未使用，保留為介面相容。""" self.mav.mav.set_position_target_global_int_send( 0, # time_boot_ms (不使用) sysid, # target_system @@ -87,4 +86,80 @@ class MavlinkSender(CommandSender): if self.mav: self.mav.close() self.mav = None - print("MavlinkSender 已關閉") \ No newline at end of file + print("MavlinkSender 已關閉") + + +class Ros2CommandSender(QObject): + """ + 透過 fc_network_apps.PositionTargetGlobalIntClient 送 goto 指令。 + + 設計重點: + - send_position_global 是同步介面，立刻回傳。結果透過 send_result signal 回報 + - 每台 drone 維護自己的 asyncio.Task pipeline，新的目標會 cancel 掉舊的 in-flight + - client 由 DroneMonitor.get_or_create_position_client 管理，per-drone 獨立節點 + """ + + # (drone_id, sysid, success, message) + send_result = pyqtSignal(str, int, bool, str) + + DEFAULT_TIMEOUT_SEC = 2.0 + + def __init__(self, monitor, timeout_sec: float = DEFAULT_TIMEOUT_SEC): + super().__init__() + self._monitor = monitor + self._timeout_sec = timeout_sec + self._pending = {} # drone_id -> asyncio.Task + + def send_position_global(self, drone_id, sysid, lat, lon, alt): + """同步介面。立刻回，結果透過 send_result signal 回報。""" + # 取消該機上一個未完成的 task，避免舊指令覆蓋新指令 + old = self._pending.get(drone_id) + if old is not None and not old.done(): + old.cancel() + + try: + loop = asyncio.get_event_loop() + except RuntimeError: + loop = asyncio.new_event_loop() + asyncio.set_event_loop(loop) + + task = loop.create_task( + self._do_send(drone_id, int(sysid), float(lat), float(lon), float(alt)) + ) + self._pending[drone_id] = task + + async def _do_send(self, drone_id, sysid, lat, lon, alt): + try: + client = self._monitor.get_or_create_position_client(drone_id) + if client is None: + self.send_result.emit( + drone_id, sysid, False, + "PositionTargetGlobalIntClient 無法建立" + ) + return + + result = await client.goto_position_only_async( + target_sysid=sysid, + target_compid=0, + latitude_deg=lat, + longitude_deg=lon, + alt=alt, + timeout_sec=self._timeout_sec, + ) + self.send_result.emit( + drone_id, sysid, bool(result.success), str(result.message or "") + ) + except asyncio.CancelledError: + # 被新的目標取代，正常狀況，不用回報 + pass + except Exception as e: + traceback.print_exc() + self.send_result.emit(drone_id, sysid, False, f"exception: {e}") + + def close(self): + """取消所有未完成的 task。PositionTargetGlobalIntClient 由 DroneMonitor 負責 destroy。""" + for task in self._pending.values(): + if not task.done(): + task.cancel() + self._pending.clear() + print("Ros2CommandSender 已關閉") \ No newline at end of file diff --git a/src/GUI/communication.py b/src/GUI/communication.py index a36810c..21263ad 100644 --- a/src/GUI/communication.py +++ b/src/GUI/communication.py @@ -36,6 +36,16 @@ except ImportError as e: traceback.print_exc() CommandLongClient = None +# 導入 fc_network_apps 的 navigation（PositionTargetGlobalInt / Offboard goto） +try: + from fc_network_apps.navigation import PositionTargetGlobalIntClient +except ImportError as e: + import traceback + print(f"⚠️ 警告: 無法導入 PositionTargetGlobalIntClient") + print(f" 错误: {e}") + traceback.print_exc() + PositionTargetGlobalIntClient = None + class DroneSignals(QObject): update_signal = pyqtSignal(str, str, object) # (msg_type, drone_id, data) @@ -475,6 +485,13 @@ class DroneMonitor(Node): self.client_counter = 0 # 用於生成唯一的 client 節點名稱 self.executor = None # 將在 gui.py 中設置，用於添加新的 clients # ================================================================================ + + # ================================================================================ + # PositionTargetGlobalIntClient 字典（per-drone，用於 Offboard goto） + # ================================================================================ + self.position_target_clients = {} # {drone_id: PositionTargetGlobalIntClient} + self.pos_client_counter = 0 + # ================================================================================ # 主题检测定时器 self.create_timer(1.0, self.scan_topics) @@ -531,7 +548,27 @@ class DroneMonitor(Node): print(f"⚠️ 無法為 {drone_id} 創建 CommandLongClient: {e}") return None return self.command_long_clients[drone_id] - + + def get_or_create_position_client(self, drone_id): + """為每個 drone 獲取或創建獨立的 PositionTargetGlobalIntClient。""" + if PositionTargetGlobalIntClient is None: + return None + with self.client_lock: + if drone_id not in self.position_target_clients: + try: + self.pos_client_counter += 1 + unique_name = f"pos_target_client_{drone_id}_{self.pos_client_counter}" + client = PositionTargetGlobalIntClient(node_name=unique_name) + self.position_target_clients[drone_id] = client + print(f" ✓ 為 {drone_id} 創建新的 PositionTargetGlobalIntClient (node={unique_name})") + if self.executor: + self.executor.add_node(client) + print(f" ✓ 將 {drone_id} 的 position client 添加到主執行器") + except Exception as e: + print(f"⚠️ 無法為 {drone_id} 創建 PositionTargetGlobalIntClient: {e}") + return None + return self.position_target_clients[drone_id] + def scan_topics(self): topics = self.get_topic_names_and_types() drone_pattern = re.compile(r'/fc_network/vehicle/(sys\d+)/(\w+)') diff --git a/src/GUI/gui.py b/src/GUI/gui.py index ef33e85..8aad4fe 100644 --- a/src/GUI/gui.py +++ b/src/GUI/gui.py @@ -25,7 +25,7 @@ from overview_table import OverviewTable # 導入任務規劃器、執行器、發送器、群組 # ================================================================================ from mission_planner import FormationPlanner, MissionType -from command_sender import MavlinkSender +from command_sender import MavlinkSender, Ros2CommandSender from mission_executor import MissionExecutor, MissionState from mission_group import ( MissionGroup, GroupPanel, DroneAssignDialog, GROUP_COLORS @@ -122,7 +122,8 @@ class ControlStationUI(QMainWindow): # ================================================================================ # 初始化指令發送器（所有群組共用） # ================================================================================ - self.command_sender = MavlinkSender("udpout:127.0.0.1:14550") # 驗證階段寫死 + # 走 ROS2 /fc_network/vehicle/pos_global_int service；per-drone client + asyncio polling + self.command_sender = Ros2CommandSender(monitor=self.monitor) # ================================================================================ # ================================================================================ @@ -671,10 +672,12 @@ class ControlStationUI(QMainWindow): executor = MissionExecutor( sender=self.command_sender, drone_gps=self.monitor.drone_gps, + monitor=self.monitor, arrival_radius=2.0, - send_rate_hz=2.0 + tick_rate_hz=2.0, ) executor.drone_waypoint_reached.connect(self.on_drone_waypoint_reached) + executor.task_status_changed.connect(self._on_task_status_changed) executor.mission_completed.connect( lambda gid=group.group_id: self._on_group_mission_completed(gid)) group.executor = executor @@ -1229,11 +1232,15 @@ class ControlStationUI(QMainWindow): if drone_gps_positions is None: return - waypoints_per_drone, center_origin = self.mission_planner.plan_formation_mission( + waypoints_per_drone, center_origin, rendezvous_indices = self.mission_planner.plan_formation_mission( drone_gps_positions, target_gps, mission_type, params=params ) - group.planned_waypoints = {'drone_ids': selected_drones, 'waypoints': waypoints_per_drone} + group.planned_waypoints = { + 'drone_ids': selected_drones, + 'waypoints': waypoints_per_drone, + 'rendezvous_indices': rendezvous_indices, + } group.center_origin = center_origin self.show_planned_waypoints(group) @@ -1300,12 +1307,16 @@ class ControlStationUI(QMainWindow): target_gps = (target_lat, target_lon, base_alt) params['rect_corners'] = rect_corners - waypoints_per_drone, center_origin = self.mission_planner.plan_formation_mission( + waypoints_per_drone, center_origin, rendezvous_indices = self.mission_planner.plan_formation_mission( drone_gps_positions, target_gps, MissionType.GRID_SWEEP, params=params ) - group.planned_waypoints = {'drone_ids': selected_drones, 'waypoints': waypoints_per_drone} + group.planned_waypoints = { + 'drone_ids': selected_drones, + 'waypoints': waypoints_per_drone, + 'rendezvous_indices': rendezvous_indices, + } group.center_origin = center_origin self.show_planned_waypoints(group) @@ -1349,6 +1360,11 @@ class ControlStationUI(QMainWindow): self.statusBar().showMessage(f"⚠ Group {group.group_id}: 請先分配無人機", 3000) return + # LEADER_FOLLOWER：把指定的領隊放到 rank 0 + leader = getattr(group, 'leader_drone_id', None) + if leader and leader in selected_drones: + selected_drones = [leader] + [d for d in selected_drones if d != leader] + print(f"路徑確認 → Group {group.group_id}: {points_json}") try: route_points = json.loads(points_json) @@ -1377,12 +1393,16 @@ class ControlStationUI(QMainWindow): target_gps = (target_lat, target_lon, base_alt) params['route_waypoints'] = route_waypoints - waypoints_per_drone, center_origin = self.mission_planner.plan_formation_mission( + waypoints_per_drone, center_origin, rendezvous_indices = self.mission_planner.plan_formation_mission( drone_gps_positions, target_gps, MissionType.LEADER_FOLLOWER, params=params ) - group.planned_waypoints = {'drone_ids': selected_drones, 'waypoints': waypoints_per_drone} + group.planned_waypoints = { + 'drone_ids': selected_drones, + 'waypoints': waypoints_per_drone, + 'rendezvous_indices': rendezvous_indices, + } group.center_origin = center_origin self.show_planned_waypoints(group) @@ -1421,6 +1441,17 @@ class ControlStationUI(QMainWindow): else: self.statusBar().showMessage(f"📍 {drone_id} → WP {wp_index}/{total}", 2000) + def _on_task_status_changed(self, drone_id, status, message): + """MissionExecutor.task_status_changed slot：把 goto 失敗/重試/fallback/barrier 丟到 status bar""" + if status == "retrying": + self.statusBar().showMessage(f"⚠ {drone_id} {message}", 4000) + elif status == "fallback_loiter": + self.statusBar().showMessage(f"❌ {drone_id} {message}", 8000) + elif status == "waiting_at_barrier": + self.statusBar().showMessage(f"⏸ {drone_id} {message}", 3000) + elif status == "normal": + self.statusBar().showMessage(f"▶ {drone_id} {message or '已恢復'}", 2000) + # ================================================================================ # 輔助方法 # ================================================================================ @@ -1747,6 +1778,12 @@ class ControlStationUI(QMainWindow): client.destroy_node() except: pass + # Clean up all PositionTargetGlobalIntClient instances + for drone_id, client in getattr(self.monitor, 'position_target_clients', {}).items(): + try: + client.destroy_node() + except: + pass self.monitor.destroy_node() self.executor.shutdown() except Exception as e: diff --git a/src/GUI/mission_executor.py b/src/GUI/mission_executor.py index b6afdff..e5bd11f 100644 --- a/src/GUI/mission_executor.py +++ b/src/GUI/mission_executor.py @@ -6,11 +6,15 @@ 設計: - 每架無人機持有一個航點序列，逐點推進 - 各自到達就各自切換到下一個航點 - - 用 QTimer 驅動，在 Qt 主線程執行 - - 暫停 = 停止發送 setpoint，飛控自動懸停 - - 相容舊的 2 階段任務與新的多航點任務 (Grid Sweep) + - 事件驅動發送：航點切換時送一次，收到 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 @@ -22,22 +26,31 @@ class MissionState(Enum): 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') + __slots__ = ( + 'drone_id', 'sysid', 'waypoints', 'wp_index', 'done', + 'sent_current_wp', 'fail_count', 'status', 'waiting_since', + ) def __init__(self, drone_id, sysid, waypoints): - """ - Args: - drone_id: GUI 用的 ID (如 's0_11') - sysid: MAVLink system ID (如 11) - waypoints: 航點序列 [(lat, lon, alt), ...] - """ 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 的瞬間 @property def current_target(self): @@ -52,7 +65,7 @@ class DroneTask: class MissionExecutor(QObject): """ - 任務執行器 + 任務執行器 (事件驅動版 + rendezvous barrier) planned_waypoints 格式: { @@ -60,31 +73,41 @@ class MissionExecutor(QObject): 'waypoints': [ [(lat,lon,alt), ...], # drone 0 [(lat,lon,alt), ...], # drone 1 - ] + ], + 'rendezvous_indices': [3, 7, ...], # 選填；缺省 = 全程不同步（各自跑） } """ - # 信號 - drone_waypoint_reached = pyqtSignal(str, int, int) # (drone_id, wp_index, total) + 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, - arrival_radius=2.0, send_rate_hz=2.0): + def __init__(self, sender, drone_gps, monitor=None, + arrival_radius=2.0, tick_rate_hz=2.0, + barrier_timeout_sec=20.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 self.state = MissionState.IDLE self.tasks = {} + self.rendezvous_indices = set() - self._interval_ms = int(1000 / send_rate_hz) + 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: print("任務已在執行中") return @@ -93,6 +116,9 @@ class MissionExecutor(QObject): 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]) @@ -102,87 +128,232 @@ class MissionExecutor(QObject): 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 (各自跑)" + ) print(f"任務啟動: {len(self.tasks)} 架無人機, " f"共 {total_wps} 個航點, " f"到達半徑={self.arrival_radius}m, " - f"發送週期={self._interval_ms}ms") + 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 print("任務暫停") def resume(self): - """恢復任務""" if self.state == MissionState.PAUSED: self._timer.start(self._interval_ms) self.state = MissionState.RUNNING print("任務恢復") def stop(self): - """停止任務""" self._timer.stop() self.tasks.clear() + self.rendezvous_indices = set() self.state = MissionState.IDLE print("任務停止") # ------------------------------------------------------------------ 控制迴圈 def _tick(self): - """控制迴圈""" - all_done = True + """ + 控制 tick： + Phase 1: 每台機做到達判定；非 barrier 點直接推進，barrier 點改成 WAITING + Phase 2: 檢查 barrier 能不能釋放（全員到齊或 timeout） + Phase 3: 每台未送過當前航點的機送一次 + Phase 4: 全部完成檢查 + """ + now = time.monotonic() + # ---- Phase 1: 到達判定 ---- for task in self.tasks.values(): - if task.done: + if task.done or task.status == TaskStatus.FALLBACK_LOITER: + continue + # 已在 barrier 等待的不再跑到達判定 + if task.status == TaskStatus.WAITING_AT_BARRIER: continue - all_done = False target = task.current_target if target is None: continue - # 讀取當前 GPS gps = self.drone_gps.get(task.drone_id) if gps is None: continue - tgt_lat, tgt_lon, tgt_alt = target + tgt_lat, tgt_lon, _ = target distance = _haversine(gps['lat'], gps['lon'], tgt_lat, tgt_lon) + if distance >= self.arrival_radius: + continue - # 到達判定 - if distance < self.arrival_radius: - task.wp_index += 1 - if task.wp_index >= task.total_waypoints: - task.done = True - self.drone_waypoint_reached.emit( - task.drone_id, task.wp_index, task.total_waypoints - ) - print(f" {task.drone_id} → DONE " - f"({task.total_waypoints}/{task.total_waypoints})") - continue - else: - self.drone_waypoint_reached.emit( - task.drone_id, task.wp_index, task.total_waypoints - ) - print(f" {task.drone_id} → WP {task.wp_index}/{task.total_waypoints} " - f"(距離: {distance:.1f}m)") - # 更新目標 - tgt_lat, tgt_lon, tgt_alt = task.current_target - - # 發送 setpoint + # 到達當前 wp_index + 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 + print(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.sysid, tgt_lat, tgt_lon, tgt_alt + 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() print("===== 任務全部完成 =====") + def _advance_waypoint(self, task, arrived_distance): + """把 task 推進一個航點，重置發送旗標。不處理 barrier 邏輯。""" + task.wp_index += 1 + task.sent_current_wp = False + task.fail_count = 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 + ) + print(f" {task.drone_id} → DONE " + f"({task.total_waypoints}/{task.total_waypoints})") + return + self.drone_waypoint_reached.emit( + task.drone_id, task.wp_index, task.total_waypoints + ) + print(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" + print(f"⚠️ barrier WP {barrier_idx} {force_reason}，強制放行") + else: + continue # 還沒到齊、也還沒 timeout → 繼續等 + + # 釋放：把所有在此 barrier 等待的機一起推進 + tag = "全員到齊" if force_reason is None else force_reason + print(f"✅ barrier WP {barrier_idx} 釋放 ({tag})，" + f"推進 {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 + print(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}" + ) + print(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: + print(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")) + # ------------------------------------------------------------------ 工具函式 @@ -196,4 +367,4 @@ def _haversine(lat1, lon1, lat2, lon2): 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)) \ No newline at end of file + return R * 2 * math.atan2(math.sqrt(a), math.sqrt(1 - a)) diff --git a/src/GUI/mission_group.py b/src/GUI/mission_group.py index 38f4322..d9a5983 100644 --- a/src/GUI/mission_group.py +++ b/src/GUI/mission_group.py @@ -35,6 +35,7 @@ class MissionGroup: self.planned_waypoints = None # 規劃結果 dict self.executor = None # MissionExecutor 實例（延遲建立） self.center_origin = None # 規劃原點 + self.leader_drone_id = None # LEADER_FOLLOWER 專用：指定的領隊無人機 ID @property def state(self): @@ -415,6 +416,21 @@ class GroupPanel(QWidget): self._param_widgets[key] = (row_w, inp) self._param_rows.append(row_w) + # LEADER_FOLLOWER 專用：領隊下拉選單 + self._leader_row = QWidget() + leader_layout = QHBoxLayout(self._leader_row) + leader_layout.setContentsMargins(0, 0, 0, 0) + leader_layout.setSpacing(3) + leader_lbl = QLabel("領隊") + leader_lbl.setStyleSheet(LBL) + self._leader_combo = QComboBox() + self._leader_combo.setStyleSheet(COMBO) + self._leader_combo.setFixedWidth(70) + self._leader_combo.currentTextChanged.connect(self._on_leader_changed) + leader_layout.addWidget(leader_lbl, 1) + leader_layout.addWidget(self._leader_combo) + param_col.addWidget(self._leader_row) + param_col.addStretch() # 初始顯示對應的參數 @@ -447,6 +463,27 @@ class GroupPanel(QWidget): self.drone_list_label.setText("、".join(sorted_ids)) self.drone_list_label.setStyleSheet( f"color: {self.group.color}; font-size: 11px; font-weight: bold;") + self._refresh_leader_options() + + def _refresh_leader_options(self): + """依目前群組成員重新填充領隊下拉選單，保留目前選擇若仍有效""" + sorted_ids = sorted(self.group.drone_ids, + key=lambda x: (x.split('_')[0], int(x.split('_')[1]))) + current = self.group.leader_drone_id + self._leader_combo.blockSignals(True) + self._leader_combo.clear() + self._leader_combo.addItems(sorted_ids) + if current and current in sorted_ids: + self._leader_combo.setCurrentText(current) + elif sorted_ids: + self._leader_combo.setCurrentText(sorted_ids[0]) + self.group.leader_drone_id = sorted_ids[0] + else: + self.group.leader_drone_id = None + self._leader_combo.blockSignals(False) + + def _on_leader_changed(self, drone_id): + self.group.leader_drone_id = drone_id if drone_id else None def update_status(self): """更新任務狀態顯示""" @@ -485,6 +522,7 @@ class GroupPanel(QWidget): visible_keys = {d[0] for d in self._param_defs.get(mission_type, [])} for key, (row_w, _inp) in self._param_widgets.items(): row_w.setVisible(key in visible_keys) + self._leader_row.setVisible(mission_type == 'LEADER_FOLLOWER') def get_mission_params(self): """讀取當前顯示的參數值，回傳 dict""" diff --git a/src/GUI/mission_planner.py b/src/GUI/mission_planner.py index 239529e..29ed44e 100644 --- a/src/GUI/mission_planner.py +++ b/src/GUI/mission_planner.py @@ -1,7 +1,7 @@ #!/usr/bin/env python3 """ 飛行任務規劃模組 -支援: M-Formation, Circle, Leader-Follower (Bezier 轉彎), Grid Sweep +支援: M-Formation, Circle, Leader-Follower (arc-length virtual leader), Grid Sweep """ import math from typing import List, Tuple, Optional, Dict, Any @@ -72,7 +72,12 @@ class FormationPlanner: mission_type: MissionType = MissionType.M_FORMATION, params: Optional[Dict[str, Any]] = None ) -> Tuple[List[List[Tuple[float, float, float]]], - Tuple[float, float, float]]: + Tuple[float, float, float], + List[int]]: + """ + 回傳 (waypoints_gps, origin, rendezvous_indices)。 + rendezvous_indices：航點序列裡該群組需要等全員到齊才推進的 index 清單。 + """ if len(drone_gps_positions) == 0: raise ValueError("無人機位置列表不能為空") @@ -89,10 +94,14 @@ class FormationPlanner: if mission_type == MissionType.M_FORMATION: s1, s2 = self._calculate_m_formation(drone_local, target_local, params) waypoints_local = [[s1[i], s2[i]] for i in range(len(drone_local))] + # 起點集合後一起出發到 stage2 + rendezvous_indices = [0] elif mission_type == MissionType.CIRCLE_FORMATION: s1, s2 = self._calculate_circle_formation(drone_local, target_local, params) waypoints_local = [[s1[i], s2[i]] for i in range(len(drone_local))] + # 半程集合後一起進最終圓環位置 + rendezvous_indices = [0] elif mission_type == MissionType.LEADER_FOLLOWER: params = params or {} @@ -103,7 +112,9 @@ class FormationPlanner: self.converter.gps_to_local(wp[0], wp[1], 0)[:2] for wp in route_wps_gps ] - waypoints_local = self._calculate_leader_follower(drone_local, route_wps_local, params) + waypoints_local, rendezvous_indices = self._calculate_leader_follower( + drone_local, route_wps_local, params + ) elif mission_type == MissionType.GRID_SWEEP: params = params or {} @@ -114,7 +125,9 @@ class FormationPlanner: self.converter.gps_to_local(c[0], c[1], 0)[:2] for c in rect_corners_gps ] - waypoints_local = self._calculate_grid_sweep(drone_local, rect_corners_local, params) + waypoints_local, rendezvous_indices = self._calculate_grid_sweep( + drone_local, rect_corners_local, params + ) else: raise ValueError(f"不支援的任務類型: {mission_type}") @@ -123,7 +136,7 @@ class FormationPlanner: gps_wps = [self.converter.local_to_gps(*wp) for wp in drone_wps] waypoints_gps.append(gps_wps) - return waypoints_gps, self.current_origin + return waypoints_gps, self.current_origin, rendezvous_indices # ------------------------------------------------------------------ M-Formation @@ -189,177 +202,321 @@ class FormationPlanner: return stage1_positions, stage2_positions - # ------------------------------------------------------------------ 路徑跟隨 (Bezier 轉彎) + # ------------------------------------------------------------------ 路徑跟隨 (虛擬領隊) def _calculate_leader_follower(self, drone_positions, route_wps_local, params): """ - 路徑跟隨編隊 — Bezier 曲線轉彎版 - - 步驟: - 1. _build_center_path: 在轉折 WP 處用二次 Bezier 曲線平滑轉彎 - 2. 固定排序，每架無人機沿中心路徑套用橫向+縱向偏移 - - 隊形 (俯視): - | (前進方向) | - | D1 | ← 左偏移, 後 0m - | D2 | ← 右偏移, 後 5m - | D3 | ← 左偏移, 後 10m - | D4 | ← 右偏移, 後 15m + 路徑跟隨編隊 — 虛擬領隊 / 弧長參數化版 (virtual leader / arc-length) + + 核心想法： + - center_path 建好後計算每個 sample 的累積弧長 s + - 每架 drone 給 (lat_k, lon_k) 的隊形偏移 + - 把 leader 想成沿 s 參數化的點，follower k 的目標 s = s_leader - lon_k + - follower k 第 i 個 waypoint = lookup(s_list[i] - lon_k) + lat_k × perp(tangent) + - 超過 path 起點/終點以 clip 處理，避免倒退或衝出 + + 解決前版「空間偏移」的三個 bug： + 1. 起點暴衝：s<0 clip → follower 起點就是 start 加橫向偏移，不往後倒退 + 2. 弧段角度爆炸：lon 不再換算成弧度，直接沿 polyline lookup + 3. straight/arc 切換不連續：統一走 polyline 線性內插與 segment 切線 + + Rank 定序規則： + **以 drone_positions 的輸入順序為準**（= GUI 選取順序）。 + drone_positions[0] = rank 0 = leader (lon=0)， + drone_positions[1] = rank 1 (lon=5)，依此類推。 + 刻意「不」用位置投影自動排序，避免浮點噪音造成 run-to-run + leader 漂移，以確保整個 mission 像軍隊縱隊那樣順序固定。 + + 隊形 (俯視, 以路徑行進方向為前): + D0 (lat=-L, lon=0) ← front-right (leader) + D1 (lat=+L, lon=5) + D2 (lat=-L, lon=10) + D3 (lat=+L, lon=15) """ N = len(drone_positions) lateral_offset = params.get('lateral_offset', 3.0) longitudinal_spacing = params.get('longitudinal_spacing', 5.0) altitude = params.get('altitude', self.base_altitude) - turn_margin = params.get('turn_margin', 0.35) # 轉彎切入距離佔段長比例 - curve_resolution = params.get('curve_resolution', 8) # 每個彎道的插值點數 - - # Step 1: 建立帶 Bezier 轉彎的中心路徑 - center_path = self._build_center_path( - route_wps_local, turn_margin, curve_resolution + min_inner_radius = params.get('min_inner_radius', 3.0) + arc_resolution = params.get('arc_resolution', 12) + sharp_turn_cos = params.get('sharp_turn_cos_threshold', 0.0) # cos(90°)=0 + + # Step 1: 建立中心路徑（含圓弧、銳角單點；每點帶累積 s） + center_path, barrier_indices = self._build_center_path( + route_wps_local, + formation_max_lateral=lateral_offset, + min_inner_radius=min_inner_radius, + arc_resolution=arc_resolution, + sharp_turn_cos_threshold=sharp_turn_cos, ) - # Step 2: 固定排序 - first_dir = (center_path[0][2], center_path[0][3]) - first_perp = (-first_dir[1], first_dir[0]) - C_x = sum(p[0] for p in drone_positions) / N - C_y = sum(p[1] for p in drone_positions) / N - - projections = [ - ((pos[0] - C_x) * first_perp[0] + (pos[1] - C_y) * first_perp[1], i) - for i, pos in enumerate(drone_positions) - ] - projections.sort() - - # Step 3: 偏移 + s_list = [pt['s'] for pt in center_path] + s_max = s_list[-1] + n_pts = len(center_path) + + def lookup(s_target): + """ + 在 center path 上以弧長 s 回傳 (x, y, tangent_dir)。 + s<0 → clip 到 start；s>s_max → clip 到 end。 + tangent 方向取當前 polyline segment 的切線，避免對不連續的 dir 做插值。 + """ + if n_pts == 1: + pt = center_path[0] + return pt['x'], pt['y'], pt['dir'] + + if s_target <= 0.0: + a = center_path[0] + b = center_path[1] + sdx, sdy = b['x'] - a['x'], b['y'] - a['y'] + slen = math.hypot(sdx, sdy) + if slen > 1e-6: + return a['x'], a['y'], (sdx / slen, sdy / slen) + return a['x'], a['y'], a['dir'] + + if s_target >= s_max: + a = center_path[-2] + b = center_path[-1] + sdx, sdy = b['x'] - a['x'], b['y'] - a['y'] + slen = math.hypot(sdx, sdy) + if slen > 1e-6: + return b['x'], b['y'], (sdx / slen, sdy / slen) + return b['x'], b['y'], b['dir'] + + # Binary search：s_list[lo] <= s_target < s_list[hi] + lo, hi = 0, n_pts - 1 + while lo + 1 < hi: + mid = (lo + hi) // 2 + if s_list[mid] <= s_target: + lo = mid + else: + hi = mid + a = center_path[lo] + b = center_path[hi] + ds = s_list[hi] - s_list[lo] + if ds < 1e-9: + return a['x'], a['y'], a['dir'] + t = (s_target - s_list[lo]) / ds + x = a['x'] + t * (b['x'] - a['x']) + y = a['y'] + t * (b['y'] - a['y']) + sdx, sdy = b['x'] - a['x'], b['y'] - a['y'] + slen = math.hypot(sdx, sdy) + if slen > 1e-6: + return x, y, (sdx / slen, sdy / slen) + return x, y, a['dir'] + + # Step 2: rank = 輸入順序（GUI 選取順序），不再做 projection sort + # 避免浮點噪音在投影值相近時翻轉 leader，保證 run-to-run 穩定 all_waypoints = [None] * N - for rank, (_, original_idx) in enumerate(projections): + # 預先算好路徑終點的切線（給收尾點用） + end = center_path[-1] + end_dx, end_dy = end['dir'] + if n_pts >= 2: + a = center_path[-2] + sdx, sdy = end['x'] - a['x'], end['y'] - a['y'] + slen = math.hypot(sdx, sdy) + if slen > 1e-6: + end_dx, end_dy = sdx / slen, sdy / slen + + for rank in range(N): lat_sign = -1 if rank % 2 == 0 else 1 lat = lat_sign * lateral_offset lon = rank * longitudinal_spacing waypoints = [] - for (cx, cy, dx, dy) in center_path: + for i in range(n_pts): + s_follower = s_list[i] - lon + x, y, (dx, dy) = lookup(s_follower) perp_x, perp_y = -dy, dx - ox = cx + lat * perp_x - lon * dx - oy = cy + lat * perp_y - lon * dy - waypoints.append((ox, oy, altitude)) + waypoints.append(( + x + lat * perp_x, + y + lat * perp_y, + altitude, + )) + + # 收尾：全員到 path 終點的 rank 位置（lon 歸零） + waypoints.append(( + end['x'] + lat * (-end_dy), + end['y'] + lat * end_dx, + altitude, + )) - all_waypoints[original_idx] = waypoints + all_waypoints[rank] = waypoints - return all_waypoints + # barrier 索引仍指向 center_path 內 (range [0, n_pts-1])， + # 不觸及末尾收尾點，直接沿用即可 + return all_waypoints, barrier_indices - def _build_center_path(self, waypoints, turn_margin, curve_resolution): + def _build_center_path(self, waypoints, + formation_max_lateral, + min_inner_radius, + arc_resolution, + sharp_turn_cos_threshold): """ - 建立帶 Bezier 曲線轉彎的中心路徑 - - 在每個轉折 WP 處: - 1. 計算 pre_turn = WP - d_in × T - 2. 計算 post_turn = WP + d_out × T - 3. 用二次 Bezier 曲線: B(t) = (1-t)²·pre + 2t(1-t)·WP + t²·post - 4. 方向從導數得到: B'(t) = 2(1-t)(WP-pre) + 2t(post-WP) - - Args: - waypoints: 路徑航點 [(x, y), ...] - turn_margin: 轉彎切入距離佔相鄰段長的比例 (0~0.5) - curve_resolution: 每個彎道的 Bezier 插值點數 + 建立以「圓弧」連接直線段的中心路徑。 + + 每個中間 waypoint 的處理： + 1. 計算入向 u_in、出向 u_out 的夾角 β = acos(u_in·u_out) + 2. 若 cos β < sharp_turn_cos_threshold → 銳角：只插單點 (hover + 原地轉向)， + barrier 放在該點，讓隊伍整體停下再繼續 + 3. 否則 → 平滑弧： + R_base = formation_max_lateral + min_inner_radius + d = R_base × tan(β/2)，並以相鄰段長的 45% 為上限 + R_actual = d / tan(β/2) + tangent points T_in = WP - u_in·d, T_out = WP + u_out·d + arc center = T_in + R_actual·sign·n_left，其中 sign=+1 左轉/CCW，-1 右轉/CW + arc 由 theta_in 到 theta_out 以 arc_resolution 等分 + path 內容: [T_in(straight,u_in), arc_samples..., T_out(straight,u_out)] + barrier 放在 T_out (轉完彎後的集合點) + + path 是一個 list[dict]，每個 dict 至少含: + {'seg': 'straight' | 'arc', + 'x': float, 'y': float, + 'dir': (dx, dy)} + 'arc' 類型額外含: 'arc_center', 'arc_R', 'arc_sign', 'theta' Returns: - [(x, y, dir_x, dir_y), ...] 中心路徑點 + 單位方向 + (path, barrier_indices) """ num_wps = len(waypoints) if num_wps < 2: - return [(waypoints[0][0], waypoints[0][1], 0.0, 1.0)] + return [{ + 'seg': 'straight', + 'x': waypoints[0][0], 'y': waypoints[0][1], + 'dir': (0.0, 1.0), + }], [] - # 計算每段方向和長度 segments = [] for i in range(num_wps - 1): dx = waypoints[i + 1][0] - waypoints[i][0] dy = waypoints[i + 1][1] - waypoints[i][1] - length = math.sqrt(dx ** 2 + dy ** 2) + length = math.hypot(dx, dy) if length < 0.01: - segments.append((0.0, 1.0, length)) + segments.append(((0.0, 1.0), length)) else: - segments.append((dx / length, dy / length, length)) + segments.append(((dx / length, dy / length), length)) + R_base = formation_max_lateral + min_inner_radius path = [] + barrier_indices = [] - # 第一個航點 - path.append((waypoints[0][0], waypoints[0][1], - segments[0][0], segments[0][1])) + # 起點 + path.append({ + 'seg': 'straight', + 'x': waypoints[0][0], 'y': waypoints[0][1], + 'dir': segments[0][0], + }) - # 中間航點 (轉彎處) for i in range(1, num_wps - 1): - d_in_x, d_in_y, len_in = segments[i - 1] - d_out_x, d_out_y, len_out = segments[i] - - # 切入距離 T: 取相鄰兩段中較短的 × turn_margin - T = min(len_in, len_out) * turn_margin - - if T < 0.5: - # 段太短，不插彎，直接加一個平均方向點 - avg_dx = d_in_x + d_out_x - avg_dy = d_in_y + d_out_y - avg_len = math.sqrt(avg_dx ** 2 + avg_dy ** 2) + u_in, len_in = segments[i - 1] + u_out, len_out = segments[i] + wp = waypoints[i] + + dot = u_in[0] * u_out[0] + u_in[1] * u_out[1] + dot = max(-1.0, min(1.0, dot)) + + # 銳角：hover + 原地轉向 + if dot < sharp_turn_cos_threshold: + avg_dx = u_in[0] + u_out[0] + avg_dy = u_in[1] + u_out[1] + avg_len = math.hypot(avg_dx, avg_dy) if avg_len > 0.01: - avg_dx /= avg_len - avg_dy /= avg_len + avg_dir = (avg_dx / avg_len, avg_dy / avg_len) else: - avg_dx, avg_dy = d_in_x, d_in_y - path.append((waypoints[i][0], waypoints[i][1], avg_dx, avg_dy)) + avg_dir = u_in + path.append({ + 'seg': 'straight', + 'x': wp[0], 'y': wp[1], + 'dir': avg_dir, + }) + barrier_indices.append(len(path) - 1) continue - # P0 = pre_turn (弧線起始) - p0_x = waypoints[i][0] - d_in_x * T - p0_y = waypoints[i][1] - d_in_y * T - - # P1 = WP 本身 (控制點) - p1_x = waypoints[i][0] - p1_y = waypoints[i][1] - - # P2 = post_turn (弧線結束) - p2_x = waypoints[i][0] + d_out_x * T - p2_y = waypoints[i][1] + d_out_y * T - - # 加入 pre_turn 點 (方向 = incoming) - path.append((p0_x, p0_y, d_in_x, d_in_y)) - - # 加入 Bezier 插值點 - for j in range(1, curve_resolution): - t = j / curve_resolution - - # B(t) = (1-t)²·P0 + 2t(1-t)·P1 + t²·P2 - one_minus_t = 1.0 - t - bx = one_minus_t * one_minus_t * p0_x + \ - 2 * t * one_minus_t * p1_x + \ - t * t * p2_x - by = one_minus_t * one_minus_t * p0_y + \ - 2 * t * one_minus_t * p1_y + \ - t * t * p2_y - - # B'(t) = 2(1-t)(P1-P0) + 2t(P2-P1) → 切線方向 - tdx = 2 * one_minus_t * (p1_x - p0_x) + 2 * t * (p2_x - p1_x) - tdy = 2 * one_minus_t * (p1_y - p0_y) + 2 * t * (p2_y - p1_y) - - # 正規化 - t_len = math.sqrt(tdx ** 2 + tdy ** 2) - if t_len > 0.01: - tdx /= t_len - tdy /= t_len - else: - tdx, tdy = d_in_x, d_in_y - - path.append((bx, by, tdx, tdy)) - - # 加入 post_turn 點 (方向 = outgoing) - path.append((p2_x, p2_y, d_out_x, d_out_y)) - - # 最後一個航點 - path.append((waypoints[-1][0], waypoints[-1][1], - segments[-1][0], segments[-1][1])) + # 平滑弧 + beta = math.acos(dot) + if beta < 1e-4: + # 幾乎直線，不插弧 + path.append({ + 'seg': 'straight', + 'x': wp[0], 'y': wp[1], + 'dir': u_in, + }) + continue - return path + half_tan = math.tan(beta / 2.0) + d_ideal = R_base * half_tan + d_max = 0.45 * min(len_in, len_out) + d = min(d_ideal, d_max) + R_actual = d / half_tan + + T_in = (wp[0] - u_in[0] * d, wp[1] - u_in[1] * d) + T_out = (wp[0] + u_out[0] * d, wp[1] + u_out[1] * d) + + # +1 CCW (左轉) / -1 CW (右轉) + cross = u_in[0] * u_out[1] - u_in[1] * u_out[0] + sign = 1 if cross > 0 else -1 + + # 入向左法線 + n_left = (-u_in[1], u_in[0]) + center = ( + T_in[0] + R_actual * sign * n_left[0], + T_in[1] + R_actual * sign * n_left[1], + ) + + theta_in = math.atan2(T_in[1] - center[1], T_in[0] - center[0]) + + # T_in (straight, 方向 u_in) + path.append({ + 'seg': 'straight', + 'x': T_in[0], 'y': T_in[1], + 'dir': u_in, + }) + + # 弧段採樣 k=1..arc_resolution-1（不含兩端） + for k in range(1, arc_resolution): + t = k / arc_resolution + theta = theta_in + sign * beta * t + px = center[0] + R_actual * math.cos(theta) + py = center[1] + R_actual * math.sin(theta) + # 切線: sign × (-sin θ, cos θ) + tx = sign * (-math.sin(theta)) + ty = sign * math.cos(theta) + path.append({ + 'seg': 'arc', + 'x': px, 'y': py, + 'dir': (tx, ty), + 'arc_center': center, + 'arc_R': R_actual, + 'arc_sign': sign, + 'theta': theta, + }) + + # T_out (straight, 方向 u_out)，barrier 放這 + path.append({ + 'seg': 'straight', + 'x': T_out[0], 'y': T_out[1], + 'dir': u_out, + }) + barrier_indices.append(len(path) - 1) + + # 終點 + path.append({ + 'seg': 'straight', + 'x': waypoints[-1][0], 'y': waypoints[-1][1], + 'dir': segments[-1][0], + }) + + # 後處理：為每個 sample 加上累積 polyline 弧長 s + path[0]['s'] = 0.0 + for i in range(1, len(path)): + prev = path[i - 1] + cur = path[i] + cur['s'] = prev['s'] + math.hypot( + cur['x'] - prev['x'], cur['y'] - prev['y'] + ) + + return path, barrier_indices # ------------------------------------------------------------------ 柵狀掃描 @@ -474,7 +631,15 @@ class FormationPlanner: all_waypoints[original_idx] = waypoints_list - return all_waypoints + # 每一條掃描線的「起點」都做一次同步： + # wp 0 = gather, wp 1 = 第一條線起點, wp 2 = 第一條線終點, + # wp 3 = 第二條線起點, wp 4 = 第二條線終點, ... + # 每條線的「起點 index」= 1, 3, 5, ... = 2*i + 1（i 從 0 開始） + # 所有 drone 的 waypoint 數量相同（num_lines 對所有 strip 都一致）， + # 所以用同一份 rendezvous_indices + rendezvous_indices = [2 * i + 1 for i in range(num_lines)] + + return all_waypoints, rendezvous_indices # ================================================================================ @@ -492,12 +657,12 @@ if __name__ == "__main__": target = (24.12400, 120.56795, 10.0) # M-Formation - wps, origin = planner.plan_formation_mission(drones, target, MissionType.M_FORMATION) + wps, origin, rv = planner.plan_formation_mission(drones, target, MissionType.M_FORMATION) print("M-Formation:") for i, wp_list in enumerate(wps): print(f" Drone {i}: {len(wp_list)} waypoints") - # Leader-Follower (Bezier 轉彎) + # Leader-Follower (虛擬領隊 / 弧長參數化) route = [ (24.12345, 120.56780), (24.12370, 120.56800), @@ -505,18 +670,18 @@ if __name__ == "__main__": (24.12400, 120.56800), (24.12420, 120.56790), ] - wps_lf, origin_lf = planner.plan_formation_mission( + wps_lf, origin_lf, rv_lf = planner.plan_formation_mission( drones, target, MissionType.LEADER_FOLLOWER, params={ 'route_waypoints': route, 'lateral_offset': 3.0, 'longitudinal_spacing': 5.0, - 'turn_margin': 0.35, - 'curve_resolution': 8, + 'min_inner_radius': 3.0, + 'arc_resolution': 8, 'altitude': 10.0 } ) - print(f"\nLeader-Follower (Bezier turns):") + print(f"\nLeader-Follower (arc-length virtual leader):") for i, wp_list in enumerate(wps_lf): print(f" Drone {i}: {len(wp_list)} waypoints") for j, wp in enumerate(wp_list): @@ -529,7 +694,7 @@ if __name__ == "__main__": (24.1240, 120.5683), (24.1240, 120.5679) ] - wps2, origin2 = planner.plan_formation_mission( + wps2, origin2, rv2 = planner.plan_formation_mission( drones, target, MissionType.GRID_SWEEP, params={'rect_corners': rect, 'line_spacing': 5.0, 'altitude': 10.0} ) diff --git a/src/GUI/validation/test_mission.py b/src/GUI/validation/test_mission.py index 22b6e22..7693789 100644 --- a/src/GUI/validation/test_mission.py +++ b/src/GUI/validation/test_mission.py @@ -210,7 +210,7 @@ def main(): target_gps = (target_lat, target_lon, BASE_ALTITUDE) print(f" 目標點: ({target_lat:.6f}, {target_lon:.6f})") - waypoints_per_drone, origin = planner.plan_formation_mission( + waypoints_per_drone, origin, _ = planner.plan_formation_mission( drone_gps_positions, target_gps, MissionType.M_FORMATION ) @@ -232,7 +232,7 @@ def main(): target_gps = (rect_center_lat, rect_center_lon, BASE_ALTITUDE) print(f" 矩形中心: ({rect_center_lat:.6f}, {rect_center_lon:.6f})") - waypoints_per_drone, origin = planner.plan_formation_mission( + waypoints_per_drone, origin, _ = planner.plan_formation_mission( drone_gps_positions, target_gps, MissionType.GRID_SWEEP, params={ 'rect_corners': rect_corners, diff --git a/src/GUI/validation/verify_waypoints.py b/src/GUI/validation/verify_waypoints.py index fe2d855..15e9356 100644 --- a/src/GUI/validation/verify_waypoints.py +++ b/src/GUI/validation/verify_waypoints.py @@ -365,7 +365,7 @@ def visualize_standalone(): fig.suptitle('Mission Planner Verification (standalone)', fontsize=14, fontweight='bold') # M-Formation - wps_m, origin_m = planner.plan_formation_mission(drones, target, MissionType.M_FORMATION) + wps_m, origin_m, _ = planner.plan_formation_mission(drones, target, MissionType.M_FORMATION) conv_m = CoordinateConverter(origin_m[0], origin_m[1], 0) data_m = { 'drone_ids': [f's0_{i + 1}' for i in range(len(drones))], @@ -380,7 +380,7 @@ def visualize_standalone(): # Grid Sweep 5m target_gs = (sum(c[0] for c in rect_corners) / 4, sum(c[1] for c in rect_corners) / 4, 10.0) - wps_g, origin_g = planner.plan_formation_mission( + wps_g, origin_g, _ = planner.plan_formation_mission( drones, target_gs, MissionType.GRID_SWEEP, params={'rect_corners': rect_corners, 'line_spacing': 5.0, 'altitude': 10.0} ) @@ -403,7 +403,7 @@ def visualize_standalone(): (24.12410, 120.56800), (24.12420, 120.56790), ] - wps_lf, origin_lf = planner.plan_formation_mission( + wps_lf, origin_lf, _ = planner.plan_formation_mission( drones, target, MissionType.LEADER_FOLLOWER, params={'route_waypoints': route, 'lateral_offset': 3.0, 'longitudinal_spacing': 5.0, 'altitude': 10.0} diff --git a/src/fc_network_apps/navigation.py b/src/fc_network_apps/navigation.py index 4e9f4c7..a094e2e 100644 --- a/src/fc_network_apps/navigation.py +++ b/src/fc_network_apps/navigation.py @@ -2,6 +2,7 @@ from __future__ import annotations +import asyncio import math from dataclasses import dataclass @@ -122,10 +123,13 @@ class PositionTargetGlobalIntClient(Node): 並等待 POSITION_TARGET_GLOBAL_INT (msg 87) 回應（由 server 端處理）。 """ - def __init__(self, service_name: str = DEFAULT_SERVICE_NAME) -> None: + def __init__(self, service_name: str = DEFAULT_SERVICE_NAME, node_name: str = None) -> None: if not rclpy.ok(): rclpy.init(args=None) - super().__init__("fc_position_target_global_int_client") + if node_name is None: + import time + node_name = f"fc_position_target_global_int_client_{int(time.time() * 1000) % 100000}" + super().__init__(node_name) self._client = self.create_client(MavPositionTargetGlobalInt, service_name) def wait_for_service(self, timeout_sec: float = 3.0) -> bool: @@ -240,7 +244,7 @@ class PositionTargetGlobalIntClient(Node): ): lat_int = int(round(latitude_deg * 1e7)) lon_int = int(round(longitude_deg * 1e7)) - + return self._send_position_target_global_int( target_sysid=target_sysid, target_compid=target_compid, @@ -260,3 +264,155 @@ class PositionTargetGlobalIntClient(Node): yaw_rate=0, timeout_sec=timeout_sec, ) + + # ============================================================================ + # 非阻塞 async 版本（asyncio polling，對齊 longCommand.py 的模板） + # 避免在 Qt 事件迴圈或主執行緒中呼叫 spin_until_future_complete + # ============================================================================ + + async def _send_position_target_global_int_async( + self, + *, + target_sysid: int, + target_compid: int, + time_boot_ms: int, + coordinate_frame: int, + type_mask: int, + lat_int: int, + lon_int: int, + alt: float, + vx: float, + vy: float, + vz: float, + afx: float, + afy: float, + afz: float, + yaw: float, + yaw_rate: float, + timeout_sec: float, + ) -> PositionTargetGlobalIntResult: + """非阻塞 async 版本 - 使用 asyncio polling 而非 spin""" + req = MavPositionTargetGlobalInt.Request() + req.target_sysid = target_sysid + req.target_compid = target_compid + req.time_boot_ms = time_boot_ms + req.coordinate_frame = coordinate_frame + req.type_mask = type_mask + req.lat_int = lat_int + req.lon_int = lon_int + req.alt = float(alt) + req.vx = float(vx) + req.vy = float(vy) + req.vz = float(vz) + req.afx = float(afx) + req.afy = float(afy) + req.afz = float(afz) + req.yaw = float(yaw) + req.yaw_rate = float(yaw_rate) + req.timeout_sec = float(timeout_sec) + + future = self._client.call_async(req) + + timeout = float(timeout_sec) + 1.0 + elapsed = 0.0 + poll_interval = 0.01 # 10ms - 與 GUI timer 頻率一致 + + while elapsed < timeout: + if future.done(): + try: + resp = future.result() + if resp is None: + return PositionTargetGlobalIntResult( + success=False, + message="Service returned None.", + ack_result=-1, + ) + if resp.ack_result != 0: + return PositionTargetGlobalIntResult( + success=False, + message=resp.message, + ack_result=resp.ack_result, + ) + echo_ok, echo_detail = _echo_position_target_matches( + type_mask=type_mask, + lat_int=lat_int, + lon_int=lon_int, + alt=alt, + vx=vx, + vy=vy, + vz=vz, + afx=afx, + afy=afy, + afz=afz, + yaw=yaw, + yaw_rate=yaw_rate, + time_boot_ms=time_boot_ms, + resp=resp, + ) + return PositionTargetGlobalIntResult( + success=echo_ok, + message=echo_detail, + ack_result=resp.ack_result, + r_time_boot_ms=int(resp.r_time_boot_ms), + r_type_mask=int(resp.r_type_mask), + r_lat_int=int(resp.r_lat_int), + r_lon_int=int(resp.r_lon_int), + r_alt=float(resp.r_alt), + r_vx=float(resp.r_vx), + r_vy=float(resp.r_vy), + r_vz=float(resp.r_vz), + r_afx=float(resp.r_afx), + r_afy=float(resp.r_afy), + r_afz=float(resp.r_afz), + r_yaw=float(resp.r_yaw), + r_yaw_rate=float(resp.r_yaw_rate), + ) + except Exception as e: + return PositionTargetGlobalIntResult( + success=False, + message=f"Error getting result: {e}", + ack_result=-1, + ) + + await asyncio.sleep(poll_interval) + elapsed += poll_interval + + return PositionTargetGlobalIntResult( + success=False, + message=f"Service timeout after {timeout}s.", + ack_result=-1, + ) + + async def goto_position_only_async( + self, + *, + target_sysid: int, + target_compid: int = 0, + latitude_deg: float, + longitude_deg: float, + alt: float = 0.0, + timeout_sec: float = DEFAULT_TIMEOUT_SEC, + ) -> PositionTargetGlobalIntResult: + """非阻塞 async 版本的 goto_position_only""" + lat_int = int(round(latitude_deg * 1e7)) + lon_int = int(round(longitude_deg * 1e7)) + + return await self._send_position_target_global_int_async( + target_sysid=target_sysid, + target_compid=target_compid, + time_boot_ms=0, + coordinate_frame=MAV_FRAME_GLOBAL_RELATIVE_ALT_INT, + type_mask=POSITION_ONLY, + lat_int=lat_int, + lon_int=lon_int, + alt=alt, + vx=0, + vy=0, + vz=0, + afx=0, + afy=0, + afz=0, + yaw=0, + yaw_rate=0, + timeout_sec=timeout_sec, + )