Update GUI 2.0.6 components and longCommand

2026-04-14 15:09:25 +08:00
parent fc2c642d6c
commit fc00ecb762
4 changed files with 227 additions and 90 deletions
@@ -29,10 +29,14 @@ class CommandLongResult:
 class CommandLongClient(Node):
    """ROS2 client : 對同一個 send_command_long service 發送各種 MAV_CMD_*。"""

-    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_command_long_client")
+        # 使用提供的 node_name，或使用帶時間戳的預設名稱以避免名稱衝突
+        if node_name is None:
+            import time
+            node_name = f"fc_command_long_client_{int(time.time() * 1000) % 100000}"
+        super().__init__(node_name)
        self._client = self.create_client(MavCommandLong, service_name)

    def wait_for_service(self, timeout_sec: float = 3.0) -> bool:
@@ -192,6 +196,83 @@ class CommandLongClient(Node):
    # 這些方法在 ThreadPoolExecutor 中運行同步版本，以避免阻塞事件循環
    # ============================================================================

+    # ============================================================================
+    # 【重新實現】正確的非阻塞 async 方法（不使用 ThreadPoolExecutor）
+    # 使用 asyncio 的 polling 機制，避免在線程中調用 spin_until_future_complete
+    # ============================================================================
+
+    async def _send_command_long_async(
+        self,
+        *,
+        target_sysid: int,
+        target_compid: int,
+        command: int,
+        confirmation: int,
+        param1: float,
+        param2: float,
+        param3: float,
+        param4: float,
+        param5: float,
+        param6: float,
+        param7: float,
+        timeout_sec: float,
+    ) -> CommandLongResult:
+        """非阻塞的 async 版本 - 使用 asyncio polling 而非 spin"""
+        req = MavCommandLong.Request()
+        req.target_sysid = target_sysid
+        req.target_compid = target_compid
+        req.command = command
+        req.confirmation = confirmation
+        req.param1 = param1
+        req.param2 = param2
+        req.param3 = param3
+        req.param4 = param4
+        req.param5 = param5
+        req.param6 = param6
+        req.param7 = param7
+        req.timeout_sec = float(timeout_sec)
+
+        # 發送異步調用
+        future = self._client.call_async(req)
+        
+        # 使用 asyncio.sleep 進行 polling，而非 spin_until_future_complete
+        # 使用 10ms 輪詢間隔以匹配 GUI 執行器的 spin_once() 頻率
+        timeout = timeout_sec + 1.0
+        elapsed = 0.0
+        poll_interval = 0.01  # 10ms - 與 GUI timer 頻率一致
+        
+        while elapsed < timeout:
+            if future.done():
+                try:
+                    response = future.result()
+                    if response is None:
+                        return CommandLongResult(
+                            success=False,
+                            message="Service returned None.",
+                            ack_result=-1,
+                        )
+                    return CommandLongResult(
+                        success=response.success,
+                        message=response.message,
+                        ack_result=response.ack_result,
+                    )
+                except Exception as e:
+                    return CommandLongResult(
+                        success=False,
+                        message=f"Error getting result: {e}",
+                        ack_result=-1,
+                    )
+            
+            # 讓出控制權給事件循環，允許 GUI executor 執行
+            await asyncio.sleep(poll_interval)
+            elapsed += poll_interval
+        
+        return CommandLongResult(
+            success=False,
+            message=f"Service timeout after {timeout}s.",
+            ack_result=-1,
+        )
+
    async def change_mode_async(
        self,
        *,
@@ -202,20 +283,20 @@ class CommandLongClient(Node):
        confirmation: int = 0,
        timeout_sec: float = DEFAULT_TIMEOUT_SEC,
    ) -> CommandLongResult:
-        """非阻塞 async 版本的 change_mode（在 ThreadPoolExecutor 中運行）"""
-        from concurrent.futures import ThreadPoolExecutor
-        loop = asyncio.get_event_loop()
-        executor = ThreadPoolExecutor(max_workers=1)
-        return await loop.run_in_executor(
-            executor,
-            lambda: self.change_mode(
-                target_sysid=target_sysid,
-                custom_mode=custom_mode,
-                target_compid=target_compid,
-                base_mode=base_mode,
-                confirmation=confirmation,
-                timeout_sec=timeout_sec,
-            )
+        """非阻塞 async 版本的 change_mode"""
+        return await self._send_command_long_async(
+            target_sysid=target_sysid,
+            target_compid=target_compid,
+            command=COMMAND_DO_SET_MODE,
+            confirmation=confirmation,
+            param1=float(base_mode),
+            param2=float(custom_mode),
+            param3=0.0,
+            param4=0.0,
+            param5=0.0,
+            param6=0.0,
+            param7=0.0,
+            timeout_sec=timeout_sec,
        )

    async def arm_disarm_async(
@@ -228,20 +309,20 @@ class CommandLongClient(Node):
        param2: float = 0.0,
        timeout_sec: float = DEFAULT_TIMEOUT_SEC,
    ) -> CommandLongResult:
-        """非阻塞 async 版本的 arm_disarm（在 ThreadPoolExecutor 中運行）"""
-        from concurrent.futures import ThreadPoolExecutor
-        loop = asyncio.get_event_loop()
-        executor = ThreadPoolExecutor(max_workers=1)
-        return await loop.run_in_executor(
-            executor,
-            lambda: self.arm_disarm(
-                target_sysid=target_sysid,
-                arm=arm,
-                target_compid=target_compid,
-                confirmation=confirmation,
-                param2=param2,
-                timeout_sec=timeout_sec,
-            )
+        """非阻塞 async 版本的 arm_disarm"""
+        return await self._send_command_long_async(
+            target_sysid=target_sysid,
+            target_compid=target_compid,
+            command=COMMAND_COMPONENT_ARM_DISARM,
+            confirmation=confirmation,
+            param1=1.0 if arm else 0.0,
+            param2=float(param2),
+            param3=0.0,
+            param4=0.0,
+            param5=0.0,
+            param6=0.0,
+            param7=0.0,
+            timeout_sec=timeout_sec,
        )

    async def takeoff_async(
@@ -256,22 +337,20 @@ class CommandLongClient(Node):
        longitude: Optional[float] = None,
        timeout_sec: float = DEFAULT_TIMEOUT_SEC,
    ) -> CommandLongResult:
-        """非阻塞 async 版本的 takeoff（在 ThreadPoolExecutor 中運行）"""
-        from concurrent.futures import ThreadPoolExecutor
-        loop = asyncio.get_event_loop()
-        executor = ThreadPoolExecutor(max_workers=1)
-        return await loop.run_in_executor(
-            executor,
-            lambda: self.takeoff(
-                target_sysid=target_sysid,
-                altitude_m=altitude_m,
-                target_compid=target_compid,
-                min_pitch_deg=min_pitch_deg,
-                yaw_deg=yaw_deg,
-                latitude=latitude,
-                longitude=longitude,
-                timeout_sec=timeout_sec,
-            )
+        """非阻塞 async 版本的 takeoff"""
+        return await self._send_command_long_async(
+            target_sysid=target_sysid,
+            target_compid=target_compid,
+            command=COMMAND_NAV_TAKEOFF,
+            confirmation=0,
+            param1=float(min_pitch_deg),
+            param2=0.0,
+            param3=0.0,
+            param4=float(yaw_deg),
+            param5=float(latitude) if latitude is not None else 0.0,
+            param6=float(longitude) if longitude is not None else 0.0,
+            param7=float(altitude_m),
+            timeout_sec=timeout_sec,
        )

    async def land_async(
@@ -285,19 +364,18 @@ class CommandLongClient(Node):
        altitude_m: float = 0.0,
        timeout_sec: float = DEFAULT_TIMEOUT_SEC,
    ) -> CommandLongResult:
-        """非阻塞 async 版本的 land（在 ThreadPoolExecutor 中運行）"""
-        from concurrent.futures import ThreadPoolExecutor
-        loop = asyncio.get_event_loop()
-        executor = ThreadPoolExecutor(max_workers=1)
-        return await loop.run_in_executor(
-            executor,
-            lambda: self.land(
-                target_sysid=target_sysid,
-                target_compid=target_compid,
-                yaw_deg=yaw_deg,
-                latitude=latitude,
-                longitude=longitude,
-                altitude_m=altitude_m,
-                timeout_sec=timeout_sec,
-            )
+        """非阻塞 async 版本的 land"""
+        return await self._send_command_long_async(
+            target_sysid=target_sysid,
+            target_compid=target_compid,
+            command=COMMAND_NAV_LAND,
+            confirmation=0,
+            param1=0.0,
+            param2=0.0,
+            param3=0.0,
+            param4=float(yaw_deg),
+            param5=float(latitude) if latitude is not None else 0.0,
+            param6=float(longitude) if longitude is not None else 0.0,
+            param7=float(altitude_m),
+            timeout_sec=timeout_sec,
        )