feat(mission): virtual-leader path following + 固定領隊 + barrier

- mission_planner: Leader-Follower 改用 arc-length 參數化(virtual
  leader),_build_center_path 以同心圓弧連接直線段,銳角退化成 hover
  單點並在該點放 rendezvous barrier,避免暴衝與編隊交叉
- mission_planner: rank 改以輸入順序為準,不再對投影排序,避免浮點噪音
  導致 run-to-run leader 漂移
- mission_group / gui: 新增 leader_drone_id 與「領隊」下拉選單,handle_
  route_confirmed 把指定領隊重排到 rank 0,整個縱隊順序固定如軍隊行進
- mission_executor: rendezvous barrier 釋放邏輯、fallback LOITER、send
  retry;加 barrier_timeout 保護
- command_sender / communication / navigation: 配合 setpoint 送達與
  Ros2 bridge 更新

Co-Authored-By: Claude Opus 4.6 <noreply@anthropic.com>
This commit is contained in:
2026-04-15 16:13:31 +08:00
co-authored by Claude Opus 4.6
parent 5c0d21fc1d
commit 0edc477df8
9 changed files with 882 additions and 203 deletions
+38 -1
View File
@@ -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+)')