"""ROS2 client for fc_network MavCommandLong service (COMMAND_LONG 系列指令).""" from __future__ import annotations from dataclasses import dataclass from typing import Optional import asyncio import rclpy from rclpy.node import Node from fc_interfaces.srv import MavCommandLong COMMAND_DO_SET_MODE = 176 COMMAND_NAV_LAND = 21 COMMAND_NAV_TAKEOFF = 22 COMMAND_COMPONENT_ARM_DISARM = 400 DEFAULT_SERVICE_NAME = "/fc_network/vehicle/send_command_long" DEFAULT_TIMEOUT_SEC = 2.0 @dataclass class CommandLongResult: success: bool message: str ack_result: int class CommandLongClient(Node): """ROS2 client : 對同一個 send_command_long service 發送各種 MAV_CMD_*。""" def __init__(self, service_name: str = DEFAULT_SERVICE_NAME, node_name: str = None) -> None: if not rclpy.ok(): rclpy.init(args=None) # 使用提供的 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: return self._client.wait_for_service(timeout_sec=timeout_sec) def _send_command_long( 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: 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) rclpy.spin_until_future_complete(self, future, timeout_sec=timeout_sec + 1.0) if not future.done() or future.result() is None: return CommandLongResult( success=False, message="Service timeout.", ack_result=-1, ) response = future.result() return CommandLongResult( success=response.success, message=response.message, ack_result=response.ack_result, ) def change_mode( self, *, target_sysid: int, custom_mode: float, target_compid: int = 0, base_mode: float = 1.0, confirmation: int = 0, timeout_sec: float = DEFAULT_TIMEOUT_SEC, ) -> CommandLongResult: return self._send_command_long( 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, ) def takeoff( self, *, target_sysid: int, altitude_m: float, target_compid: int = 0, min_pitch_deg: float = 0.0, yaw_deg: float = 0.0, latitude: Optional[float] = None, longitude: Optional[float] = None, timeout_sec: float = DEFAULT_TIMEOUT_SEC, ) -> CommandLongResult: return self._send_command_long( 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, ) def land( self, *, target_sysid: int, target_compid: int = 0, yaw_deg: float = 0.0, latitude: Optional[float] = None, longitude: Optional[float] = None, altitude_m: float = 0.0, timeout_sec: float = DEFAULT_TIMEOUT_SEC, ) -> CommandLongResult: return self._send_command_long( 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, ) def arm_disarm( self, *, target_sysid: int, arm: bool, target_compid: int = 0, confirmation: int = 0, param2: float = 0.0, timeout_sec: float = DEFAULT_TIMEOUT_SEC, ) -> CommandLongResult: return self._send_command_long( 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 包裝方法（用於 GUI 的非阻塞調用） # 這些方法在 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, *, target_sysid: int, custom_mode: float, target_compid: int = 0, base_mode: float = 1.0, confirmation: int = 0, timeout_sec: float = DEFAULT_TIMEOUT_SEC, ) -> CommandLongResult: """非阻塞 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( self, *, target_sysid: int, arm: bool, target_compid: int = 0, confirmation: int = 0, param2: float = 0.0, timeout_sec: float = DEFAULT_TIMEOUT_SEC, ) -> CommandLongResult: """非阻塞 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( self, *, target_sysid: int, altitude_m: float, target_compid: int = 0, min_pitch_deg: float = 0.0, yaw_deg: float = 0.0, latitude: Optional[float] = None, longitude: Optional[float] = None, timeout_sec: float = DEFAULT_TIMEOUT_SEC, ) -> CommandLongResult: """非阻塞 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( self, *, target_sysid: int, target_compid: int = 0, yaw_deg: float = 0.0, latitude: Optional[float] = None, longitude: Optional[float] = None, altitude_m: float = 0.0, timeout_sec: float = DEFAULT_TIMEOUT_SEC, ) -> CommandLongResult: """非阻塞 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, )