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>
2026-04-15 16:13:31 +08:00
parent 5c0d21fc1d
commit 0edc477df8
9 changed files with 882 additions and 203 deletions
@@ -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,
+        )