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:
@@ -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+)')
|
||||
|
||||
Reference in New Issue
Block a user