From 415aed088a975c5c7dd69899c4484a00e40a6bc2 Mon Sep 17 00:00:00 2001 From: wenchun Date: Mon, 8 Jun 2026 16:51:19 +0800 Subject: [PATCH] =?UTF-8?q?fix:=20=E4=BF=AE=E6=AD=A3=E5=AF=A6=E9=A3=9B=20C?= =?UTF-8?q?ircle=20=E9=AB=98=E5=BA=A6=E7=88=86=E8=A1=9D=E8=88=87=20Leader-?= =?UTF-8?q?Follower=20=E5=8D=A1=E9=BB=9E?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 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 --- src/GUI/gui.py | 19 +++++- src/GUI/mission_executor.py | 59 +++++++++++++++++-- src/GUI/mission_group.py | 50 +++++++++++++++- src/GUI/mission_planner.py | 6 +- .../fc_network_adapter/mavlinkROS2Nodes.py | 4 +- 5 files changed, 125 insertions(+), 13 deletions(-) diff --git a/src/GUI/gui.py b/src/GUI/gui.py index 913981f..ec7283a 100644 --- a/src/GUI/gui.py +++ b/src/GUI/gui.py @@ -1310,12 +1310,18 @@ class ControlStationUI(QMainWindow): self.drone_map.set_mission_mode(mission_type) def _create_executor_for_group(self, group): - """為群組建立 MissionExecutor""" + """為群組建立 MissionExecutor，讀取 panel 上的執行參數""" + exec_params = {} + panel = self.group_panels.get(group.group_id) + if panel and hasattr(panel, 'get_execution_params'): + exec_params = panel.get_execution_params() + executor = MissionExecutor( sender=self.command_sender, drone_gps=self.monitor.drone_gps, monitor=self.monitor, - arrival_radius=2.0, + arrival_radius=exec_params.get('arrival_radius', 4.0), + hover_stable_sec=exec_params.get('hover_stable_sec', 2.0), tick_rate_hz=2.0, ) executor.drone_waypoint_reached.connect(self.on_drone_waypoint_reached) @@ -1334,6 +1340,15 @@ class ControlStationUI(QMainWindow): return if group.executor is None: self._create_executor_for_group(group) + else: + # 已存在的 executor 也要重讀執行參數，讓使用者調整能生效 + panel = self.group_panels.get(group_id) + if panel and hasattr(panel, 'get_execution_params'): + ep = panel.get_execution_params() + group.executor.arrival_radius = ep.get( + 'arrival_radius', group.executor.arrival_radius) + group.executor.hover_stable_sec = ep.get( + 'hover_stable_sec', group.executor.hover_stable_sec) group.executor.start(group.planned_waypoints) panel = self.group_panels.get(group_id) if panel: diff --git a/src/GUI/mission_executor.py b/src/GUI/mission_executor.py index e086362..2104925 100644 --- a/src/GUI/mission_executor.py +++ b/src/GUI/mission_executor.py @@ -43,6 +43,7 @@ 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): @@ -55,6 +56,8 @@ class DroneTask: 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): @@ -89,14 +92,21 @@ class MissionExecutor(QObject): mission_completed = pyqtSignal() def __init__(self, sender, drone_gps, monitor=None, - arrival_radius=2.0, tick_rate_hz=2.0, - barrier_timeout_sec=20.0): + 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() @@ -140,7 +150,7 @@ class MissionExecutor(QObject): "INFO", f"任務已啟動: {len(self.tasks)} 架無人機, " f"共 {total_wps} 個航點, " - f"到達半徑={self.arrival_radius}m, " + 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}", @@ -177,7 +187,7 @@ class MissionExecutor(QObject): """ now = time.monotonic() - # ---- Phase 1: 到達判定 ---- + # ---- Phase 1: 到達判定（hover-stable） ---- for task in self.tasks.values(): if task.done or task.status == TaskStatus.FALLBACK_LOITER: continue @@ -195,10 +205,32 @@ class MissionExecutor(QObject): tgt_lat, tgt_lon, _ = target distance = _haversine(gps['lat'], gps['lon'], tgt_lat, tgt_lon) - if distance >= self.arrival_radius: + + # 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 等待 @@ -245,11 +277,28 @@ class MissionExecutor(QObject): 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( diff --git a/src/GUI/mission_group.py b/src/GUI/mission_group.py index 17f79aa..2a5122b 100644 --- a/src/GUI/mission_group.py +++ b/src/GUI/mission_group.py @@ -32,9 +32,16 @@ DEFAULT_MISSION_PARAM_VALUES = { 'lateral_offset': '3.0', 'longitudinal_spacing': '5.0', 'line_spacing': '5.0', + # 執行參數（所有任務類型共用） + 'arrival_radius': '4.0', # 到達半徑 (m)，實飛建議 3~5，SITL 可調小到 2 + 'hover_stable_sec': '2.0', # 在 radius 內穩定停留多久才視為到達 (s) } +# 執行參數的 key 集合（所有 mission type 共用，不隨類型切換） +EXECUTION_PARAM_KEYS = ('arrival_radius', 'hover_stable_sec') + + class MissionGroup: """單一任務群組的資料""" @@ -424,6 +431,12 @@ class GroupPanel(QWidget): ], } + # 所有 mission type 共用的執行參數（不隨類型切換隱藏） + self._exec_param_defs = [ + ('arrival_radius', '到達半徑 (m)', self.default_params['arrival_radius']), + ('hover_stable_sec', '停留判定 (s)', self.default_params['hover_stable_sec']), + ] + # 建立所有參數列的 widget（先全部建好，切換時顯示/隱藏） self._param_widgets = {} # key → (label_widget, input_widget) self._param_rows = [] # 所有 row layout 對應的 container widget @@ -447,6 +460,23 @@ class GroupPanel(QWidget): self._param_widgets[key] = (row_w, inp) self._param_rows.append(row_w) + # 執行參數列（所有 mission type 共用，永遠顯示） + for key, label_text, default in self._exec_param_defs: + row_w = QWidget() + row_l = QHBoxLayout(row_w) + row_l.setContentsMargins(0, 0, 0, 0) + row_l.setSpacing(3) + lbl = QLabel(label_text) + lbl.setStyleSheet(LBL) + inp = QLineEdit(default) + inp.setStyleSheet(INPUT) + inp.setFixedWidth(50) + row_l.addWidget(lbl, 1) + row_l.addWidget(inp) + param_col.addWidget(row_w) + 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) @@ -546,15 +576,17 @@ class GroupPanel(QWidget): self.delete_group_btn.setEnabled(enabled) def _update_param_visibility(self, _=None): - """根據當前任務類型，顯示/隱藏對應的參數列""" + """根據當前任務類型，顯示/隱藏對應的參數列。執行參數永遠顯示。""" mission_type = self.type_combo.currentText() - visible_keys = {d[0] for d in self._param_defs.get(mission_type, [])} + mission_keys = {d[0] for d in self._param_defs.get(mission_type, [])} + exec_keys = {d[0] for d in self._exec_param_defs} + visible_keys = mission_keys | exec_keys 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""" + """讀取當前任務類型的規劃參數，回傳 dict（不含執行參數）""" mission_type = self.type_combo.currentText() params = {} for key, _label, default in self._param_defs.get(mission_type, []): @@ -566,6 +598,18 @@ class GroupPanel(QWidget): params[key] = float(default) return params + def get_execution_params(self): + """讀取執行參數（arrival_radius, hover_stable_sec 等），回傳 dict""" + params = {} + for key, _label, default in self._exec_param_defs: + if key in self._param_widgets: + _row_w, inp = self._param_widgets[key] + try: + params[key] = float(inp.text()) + except ValueError: + params[key] = float(default) + return params + def set_param_value(self, key, value): """更新指定參數欄位的文字值。""" if key not in self._param_widgets: diff --git a/src/GUI/mission_planner.py b/src/GUI/mission_planner.py index c43d2c2..7530356 100644 --- a/src/GUI/mission_planner.py +++ b/src/GUI/mission_planner.py @@ -193,11 +193,13 @@ class FormationPlanner: final_y = center_y + radius * math.sin(angle_rad) final_z = altitude - current_x, current_y, current_z = drone_positions[i] + current_x, current_y, _current_z = drone_positions[i] + # stage1 高度直接用 final_z：先爬升到目標高度再水平集合， + # 避免用 current_z 做中點插值（current_z 在實飛時可能是 AMSL 而非相對 home，會導致衝高） stage1_positions.append(( current_x + (final_x - current_x) * 0.5, current_y + (final_y - current_y) * 0.5, - current_z + (final_z - current_z) * 0.5 + final_z, )) stage2_positions.append((final_x, final_y, final_z)) diff --git a/src/fc_network_adapter/fc_network_adapter/mavlinkROS2Nodes.py b/src/fc_network_adapter/fc_network_adapter/mavlinkROS2Nodes.py index 6948673..1cfd3b9 100644 --- a/src/fc_network_adapter/fc_network_adapter/mavlinkROS2Nodes.py +++ b/src/fc_network_adapter/fc_network_adapter/mavlinkROS2Nodes.py @@ -230,7 +230,9 @@ class VehicleStatusPublisher(Node): msg.stamp = self.get_clock().now().to_msg() msg.latitude = float(pos.latitude) msg.longitude = float(pos.longitude) - msg.altitude = float(pos.altitude) if pos.altitude is not None else 0.0 + # 發布「相對 home 高度」而非 AMSL：下游 GUI / mission planner 一律以相對高度做運算與指令， + # 若這裡發 AMSL（msg.alt）會在實飛時把海拔當成相對高度傳給飛控，造成 Circle 高度衝高的 bug + msg.altitude = float(pos.relative_altitude) if pos.relative_altitude is not None else 0.0 # GPS 狀態資訊 gps = status.gps