|
|
|
|
@ -4,6 +4,7 @@ from __future__ import annotations
|
|
|
|
|
|
|
|
|
|
from dataclasses import dataclass
|
|
|
|
|
from typing import Optional
|
|
|
|
|
import asyncio
|
|
|
|
|
|
|
|
|
|
import rclpy
|
|
|
|
|
from rclpy.node import Node
|
|
|
|
|
@ -185,3 +186,118 @@ class CommandLongClient(Node):
|
|
|
|
|
param7=0.0,
|
|
|
|
|
timeout_sec=timeout_sec,
|
|
|
|
|
)
|
|
|
|
|
|
|
|
|
|
# ============================================================================
|
|
|
|
|
# 【新增】非阻塞 async 包裝方法(用於 GUI 的非阻塞調用)
|
|
|
|
|
# 這些方法在 ThreadPoolExecutor 中運行同步版本,以避免阻塞事件循環
|
|
|
|
|
# ============================================================================
|
|
|
|
|
|
|
|
|
|
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(在 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 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(在 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 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(在 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 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(在 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,
|
|
|
|
|
)
|
|
|
|
|
)
|
|
|
|
|
|