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:
Claude Opus 4.6
+88
-13
@@ -3,10 +3,16 @@
|
||||
指令發送模組
|
||||
負責將目標位置轉成具體的通訊指令發送到飛控
|
||||
|
||||
現階段: MavlinkSender (直接 pymavlink 發送)
|
||||
未來: 替換為 ROS2Sender (發到 ROS2 topic，由 fc_network_adapter 轉發)
|
||||
目前使用 Ros2CommandSender，經 fc_network_adapter 的
|
||||
/fc_network/vehicle/pos_global_int service 發送。
|
||||
|
||||
MavlinkSender 保留作為直連 SITL 的 fallback 工具，但已不是預設路徑。
|
||||
"""
|
||||
import asyncio
|
||||
import traceback
|
||||
from abc import ABC, abstractmethod
|
||||
|
||||
from PyQt6.QtCore import QObject, pyqtSignal
|
||||
from pymavlink import mavutil
|
||||
|
||||
|
||||
@@ -14,11 +20,12 @@ class CommandSender(ABC):
|
||||
"""指令發送抽象介面"""
|
||||
|
||||
@abstractmethod
|
||||
def send_position_global(self, sysid, lat, lon, alt):
|
||||
def send_position_global(self, drone_id, sysid, lat, lon, alt):
|
||||
"""
|
||||
發送全球座標位置指令
|
||||
|
||||
Args:
|
||||
drone_id: GUI 用的 drone 識別字串 (如 's0_11')
|
||||
sysid: 目標無人機的 MAVLink system ID
|
||||
lat: 緯度 (度)
|
||||
lon: 經度 (度)
|
||||
@@ -58,16 +65,8 @@ class MavlinkSender(CommandSender):
|
||||
self.mav = mavutil.mavlink_connection(connection_string)
|
||||
print(f"MavlinkSender 已建立連線: {connection_string}")
|
||||
|
||||
def send_position_global(self, sysid, lat, lon, alt):
|
||||
"""
|
||||
發送 SET_POSITION_TARGET_GLOBAL_INT
|
||||
|
||||
注意:
|
||||
- coordinate_frame 使用 MAV_FRAME_GLOBAL_RELATIVE_ALT_INT
|
||||
高度是相對起飛點的高度 (公尺)
|
||||
- 如果 FormationPlanner 產出的 alt 是 AMSL 絕對高度，
|
||||
需要在外部先減去起飛點高度再傳入
|
||||
"""
|
||||
def send_position_global(self, drone_id, sysid, lat, lon, alt):
|
||||
"""發送 SET_POSITION_TARGET_GLOBAL_INT。drone_id 未使用，保留為介面相容。"""
|
||||
self.mav.mav.set_position_target_global_int_send(
|
||||
0, # time_boot_ms (不使用)
|
||||
sysid, # target_system
|
||||
@@ -88,3 +87,79 @@ class MavlinkSender(CommandSender):
|
||||
self.mav.close()
|
||||
self.mav = None
|
||||
print("MavlinkSender 已關閉")
|
||||
|
||||
|
||||
class Ros2CommandSender(QObject):
|
||||
"""
|
||||
透過 fc_network_apps.PositionTargetGlobalIntClient 送 goto 指令。
|
||||
|
||||
設計重點:
|
||||
- send_position_global 是同步介面，立刻回傳。結果透過 send_result signal 回報
|
||||
- 每台 drone 維護自己的 asyncio.Task pipeline，新的目標會 cancel 掉舊的 in-flight
|
||||
- client 由 DroneMonitor.get_or_create_position_client 管理，per-drone 獨立節點
|
||||
"""
|
||||
|
||||
# (drone_id, sysid, success, message)
|
||||
send_result = pyqtSignal(str, int, bool, str)
|
||||
|
||||
DEFAULT_TIMEOUT_SEC = 2.0
|
||||
|
||||
def __init__(self, monitor, timeout_sec: float = DEFAULT_TIMEOUT_SEC):
|
||||
super().__init__()
|
||||
self._monitor = monitor
|
||||
self._timeout_sec = timeout_sec
|
||||
self._pending = {} # drone_id -> asyncio.Task
|
||||
|
||||
def send_position_global(self, drone_id, sysid, lat, lon, alt):
|
||||
"""同步介面。立刻回，結果透過 send_result signal 回報。"""
|
||||
# 取消該機上一個未完成的 task，避免舊指令覆蓋新指令
|
||||
old = self._pending.get(drone_id)
|
||||
if old is not None and not old.done():
|
||||
old.cancel()
|
||||
|
||||
try:
|
||||
loop = asyncio.get_event_loop()
|
||||
except RuntimeError:
|
||||
loop = asyncio.new_event_loop()
|
||||
asyncio.set_event_loop(loop)
|
||||
|
||||
task = loop.create_task(
|
||||
self._do_send(drone_id, int(sysid), float(lat), float(lon), float(alt))
|
||||
)
|
||||
self._pending[drone_id] = task
|
||||
|
||||
async def _do_send(self, drone_id, sysid, lat, lon, alt):
|
||||
try:
|
||||
client = self._monitor.get_or_create_position_client(drone_id)
|
||||
if client is None:
|
||||
self.send_result.emit(
|
||||
drone_id, sysid, False,
|
||||
"PositionTargetGlobalIntClient 無法建立"
|
||||
)
|
||||
return
|
||||
|
||||
result = await client.goto_position_only_async(
|
||||
target_sysid=sysid,
|
||||
target_compid=0,
|
||||
latitude_deg=lat,
|
||||
longitude_deg=lon,
|
||||
alt=alt,
|
||||
timeout_sec=self._timeout_sec,
|
||||
)
|
||||
self.send_result.emit(
|
||||
drone_id, sysid, bool(result.success), str(result.message or "")
|
||||
)
|
||||
except asyncio.CancelledError:
|
||||
# 被新的目標取代，正常狀況，不用回報
|
||||
pass
|
||||
except Exception as e:
|
||||
traceback.print_exc()
|
||||
self.send_result.emit(drone_id, sysid, False, f"exception: {e}")
|
||||
|
||||
def close(self):
|
||||
"""取消所有未完成的 task。PositionTargetGlobalIntClient 由 DroneMonitor 負責 destroy。"""
|
||||
for task in self._pending.values():
|
||||
if not task.done():
|
||||
task.cancel()
|
||||
self._pending.clear()
|
||||
print("Ros2CommandSender 已關閉")
|
||||
@@ -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)
|
||||
|
||||
@@ -476,6 +486,13 @@ class DroneMonitor(Node):
|
||||
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)
|
||||
|
||||
@@ -532,6 +549,26 @@ class DroneMonitor(Node):
|
||||
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+)')
|
||||
|
||||
+46
-9
@@ -25,7 +25,7 @@ from overview_table import OverviewTable
|
||||
# 導入任務規劃器、執行器、發送器、群組
|
||||
# ================================================================================
|
||||
from mission_planner import FormationPlanner, MissionType
|
||||
from command_sender import MavlinkSender
|
||||
from command_sender import MavlinkSender, Ros2CommandSender
|
||||
from mission_executor import MissionExecutor, MissionState
|
||||
from mission_group import (
|
||||
MissionGroup, GroupPanel, DroneAssignDialog, GROUP_COLORS
|
||||
@@ -122,7 +122,8 @@ class ControlStationUI(QMainWindow):
|
||||
# ================================================================================
|
||||
# 初始化指令發送器（所有群組共用）
|
||||
# ================================================================================
|
||||
self.command_sender = MavlinkSender("udpout:127.0.0.1:14550") # 驗證階段寫死
|
||||
# 走 ROS2 /fc_network/vehicle/pos_global_int service；per-drone client + asyncio polling
|
||||
self.command_sender = Ros2CommandSender(monitor=self.monitor)
|
||||
# ================================================================================
|
||||
|
||||
# ================================================================================
|
||||
@@ -671,10 +672,12 @@ class ControlStationUI(QMainWindow):
|
||||
executor = MissionExecutor(
|
||||
sender=self.command_sender,
|
||||
drone_gps=self.monitor.drone_gps,
|
||||
monitor=self.monitor,
|
||||
arrival_radius=2.0,
|
||||
send_rate_hz=2.0
|
||||
tick_rate_hz=2.0,
|
||||
)
|
||||
executor.drone_waypoint_reached.connect(self.on_drone_waypoint_reached)
|
||||
executor.task_status_changed.connect(self._on_task_status_changed)
|
||||
executor.mission_completed.connect(
|
||||
lambda gid=group.group_id: self._on_group_mission_completed(gid))
|
||||
group.executor = executor
|
||||
@@ -1229,11 +1232,15 @@ class ControlStationUI(QMainWindow):
|
||||
if drone_gps_positions is None:
|
||||
return
|
||||
|
||||
waypoints_per_drone, center_origin = self.mission_planner.plan_formation_mission(
|
||||
waypoints_per_drone, center_origin, rendezvous_indices = self.mission_planner.plan_formation_mission(
|
||||
drone_gps_positions, target_gps, mission_type, params=params
|
||||
)
|
||||
|
||||
group.planned_waypoints = {'drone_ids': selected_drones, 'waypoints': waypoints_per_drone}
|
||||
group.planned_waypoints = {
|
||||
'drone_ids': selected_drones,
|
||||
'waypoints': waypoints_per_drone,
|
||||
'rendezvous_indices': rendezvous_indices,
|
||||
}
|
||||
group.center_origin = center_origin
|
||||
self.show_planned_waypoints(group)
|
||||
|
||||
@@ -1300,12 +1307,16 @@ class ControlStationUI(QMainWindow):
|
||||
target_gps = (target_lat, target_lon, base_alt)
|
||||
|
||||
params['rect_corners'] = rect_corners
|
||||
waypoints_per_drone, center_origin = self.mission_planner.plan_formation_mission(
|
||||
waypoints_per_drone, center_origin, rendezvous_indices = self.mission_planner.plan_formation_mission(
|
||||
drone_gps_positions, target_gps, MissionType.GRID_SWEEP,
|
||||
params=params
|
||||
)
|
||||
|
||||
group.planned_waypoints = {'drone_ids': selected_drones, 'waypoints': waypoints_per_drone}
|
||||
group.planned_waypoints = {
|
||||
'drone_ids': selected_drones,
|
||||
'waypoints': waypoints_per_drone,
|
||||
'rendezvous_indices': rendezvous_indices,
|
||||
}
|
||||
group.center_origin = center_origin
|
||||
self.show_planned_waypoints(group)
|
||||
|
||||
@@ -1349,6 +1360,11 @@ class ControlStationUI(QMainWindow):
|
||||
self.statusBar().showMessage(f"⚠ Group {group.group_id}: 請先分配無人機", 3000)
|
||||
return
|
||||
|
||||
# LEADER_FOLLOWER：把指定的領隊放到 rank 0
|
||||
leader = getattr(group, 'leader_drone_id', None)
|
||||
if leader and leader in selected_drones:
|
||||
selected_drones = [leader] + [d for d in selected_drones if d != leader]
|
||||
|
||||
print(f"路徑確認 → Group {group.group_id}: {points_json}")
|
||||
try:
|
||||
route_points = json.loads(points_json)
|
||||
@@ -1377,12 +1393,16 @@ class ControlStationUI(QMainWindow):
|
||||
target_gps = (target_lat, target_lon, base_alt)
|
||||
|
||||
params['route_waypoints'] = route_waypoints
|
||||
waypoints_per_drone, center_origin = self.mission_planner.plan_formation_mission(
|
||||
waypoints_per_drone, center_origin, rendezvous_indices = self.mission_planner.plan_formation_mission(
|
||||
drone_gps_positions, target_gps, MissionType.LEADER_FOLLOWER,
|
||||
params=params
|
||||
)
|
||||
|
||||
group.planned_waypoints = {'drone_ids': selected_drones, 'waypoints': waypoints_per_drone}
|
||||
group.planned_waypoints = {
|
||||
'drone_ids': selected_drones,
|
||||
'waypoints': waypoints_per_drone,
|
||||
'rendezvous_indices': rendezvous_indices,
|
||||
}
|
||||
group.center_origin = center_origin
|
||||
self.show_planned_waypoints(group)
|
||||
|
||||
@@ -1421,6 +1441,17 @@ class ControlStationUI(QMainWindow):
|
||||
else:
|
||||
self.statusBar().showMessage(f"📍 {drone_id} → WP {wp_index}/{total}", 2000)
|
||||
|
||||
def _on_task_status_changed(self, drone_id, status, message):
|
||||
"""MissionExecutor.task_status_changed slot：把 goto 失敗/重試/fallback/barrier 丟到 status bar"""
|
||||
if status == "retrying":
|
||||
self.statusBar().showMessage(f"⚠ {drone_id} {message}", 4000)
|
||||
elif status == "fallback_loiter":
|
||||
self.statusBar().showMessage(f"❌ {drone_id} {message}", 8000)
|
||||
elif status == "waiting_at_barrier":
|
||||
self.statusBar().showMessage(f"⏸ {drone_id} {message}", 3000)
|
||||
elif status == "normal":
|
||||
self.statusBar().showMessage(f"▶ {drone_id} {message or '已恢復'}", 2000)
|
||||
|
||||
# ================================================================================
|
||||
# 輔助方法
|
||||
# ================================================================================
|
||||
@@ -1747,6 +1778,12 @@ class ControlStationUI(QMainWindow):
|
||||
client.destroy_node()
|
||||
except:
|
||||
pass
|
||||
# Clean up all PositionTargetGlobalIntClient instances
|
||||
for drone_id, client in getattr(self.monitor, 'position_target_clients', {}).items():
|
||||
try:
|
||||
client.destroy_node()
|
||||
except:
|
||||
pass
|
||||
self.monitor.destroy_node()
|
||||
self.executor.shutdown()
|
||||
except Exception as e:
|
||||
|
||||
+214
-43
@@ -6,11 +6,15 @@
|
||||
設計:
|
||||
- 每架無人機持有一個航點序列，逐點推進
|
||||
- 各自到達就各自切換到下一個航點
|
||||
- 用 QTimer 驅動，在 Qt 主線程執行
|
||||
- 暫停 = 停止發送 setpoint，飛控自動懸停
|
||||
- 相容舊的 2 階段任務與新的多航點任務 (Grid Sweep)
|
||||
- 事件驅動發送：航點切換時送一次，收到 send_result 才決定重送/前進
|
||||
- 失敗策略 (b)：單機重試 MAX_RETRY 次仍失敗 → 該機 fallback LOITER，其他機繼續
|
||||
- Rendezvous barrier：在指定 wp index 等所有活著的機到齊才一起推進，有 timeout 保護
|
||||
- 用 QTimer 驅動到達判定，在 Qt 主線程執行
|
||||
- 暫停 = 停止到達判定 + 停止新指令
|
||||
"""
|
||||
import asyncio
|
||||
import math
|
||||
import time
|
||||
from enum import Enum
|
||||
from PyQt6.QtCore import QObject, QTimer, pyqtSignal
|
||||
|
||||
@@ -22,22 +26,31 @@ class MissionState(Enum):
|
||||
PAUSED = "paused"
|
||||
|
||||
|
||||
class TaskStatus(Enum):
|
||||
"""單機任務狀態"""
|
||||
NORMAL = "normal"
|
||||
RETRYING = "retrying"
|
||||
WAITING_AT_BARRIER = "waiting_at_barrier"
|
||||
FALLBACK_LOITER = "fallback_loiter"
|
||||
|
||||
|
||||
class DroneTask:
|
||||
"""單架無人機的任務資料"""
|
||||
__slots__ = ('drone_id', 'sysid', 'waypoints', 'wp_index', 'done')
|
||||
__slots__ = (
|
||||
'drone_id', 'sysid', 'waypoints', 'wp_index', 'done',
|
||||
'sent_current_wp', 'fail_count', 'status', 'waiting_since',
|
||||
)
|
||||
|
||||
def __init__(self, drone_id, sysid, waypoints):
|
||||
"""
|
||||
Args:
|
||||
drone_id: GUI 用的 ID (如 's0_11')
|
||||
sysid: MAVLink system ID (如 11)
|
||||
waypoints: 航點序列 [(lat, lon, alt), ...]
|
||||
"""
|
||||
self.drone_id = drone_id
|
||||
self.sysid = sysid
|
||||
self.waypoints = waypoints
|
||||
self.wp_index = 0
|
||||
self.done = len(waypoints) == 0
|
||||
self.sent_current_wp = False
|
||||
self.fail_count = 0
|
||||
self.status = TaskStatus.NORMAL
|
||||
self.waiting_since = 0.0 # monotonic time 進入 WAITING_AT_BARRIER 的瞬間
|
||||
|
||||
@property
|
||||
def current_target(self):
|
||||
@@ -52,7 +65,7 @@ class DroneTask:
|
||||
|
||||
class MissionExecutor(QObject):
|
||||
"""
|
||||
任務執行器
|
||||
任務執行器 (事件驅動版 + rendezvous barrier)
|
||||
|
||||
planned_waypoints 格式:
|
||||
{
|
||||
@@ -60,31 +73,41 @@ class MissionExecutor(QObject):
|
||||
'waypoints': [
|
||||
[(lat,lon,alt), ...], # drone 0
|
||||
[(lat,lon,alt), ...], # drone 1
|
||||
]
|
||||
],
|
||||
'rendezvous_indices': [3, 7, ...], # 選填；缺省 = 全程不同步（各自跑）
|
||||
}
|
||||
"""
|
||||
|
||||
# 信號
|
||||
MAX_RETRY = 3
|
||||
|
||||
drone_waypoint_reached = pyqtSignal(str, int, int) # (drone_id, wp_index, total)
|
||||
task_status_changed = pyqtSignal(str, str, str) # (drone_id, status, message)
|
||||
mission_completed = pyqtSignal()
|
||||
|
||||
def __init__(self, sender, drone_gps,
|
||||
arrival_radius=2.0, send_rate_hz=2.0):
|
||||
def __init__(self, sender, drone_gps, monitor=None,
|
||||
arrival_radius=2.0, tick_rate_hz=2.0,
|
||||
barrier_timeout_sec=20.0):
|
||||
super().__init__()
|
||||
self.sender = sender
|
||||
self.drone_gps = drone_gps
|
||||
self.monitor = monitor # 用於失敗 fallback 到 LOITER
|
||||
self.arrival_radius = arrival_radius
|
||||
self.barrier_timeout_sec = barrier_timeout_sec
|
||||
self.state = MissionState.IDLE
|
||||
self.tasks = {}
|
||||
self.rendezvous_indices = set()
|
||||
|
||||
self._interval_ms = int(1000 / send_rate_hz)
|
||||
self._interval_ms = int(1000 / tick_rate_hz)
|
||||
self._timer = QTimer()
|
||||
self._timer.timeout.connect(self._tick)
|
||||
|
||||
# 只有 Ros2CommandSender 有 send_result signal；MavlinkSender 沒有
|
||||
if hasattr(sender, 'send_result'):
|
||||
sender.send_result.connect(self._on_send_result)
|
||||
|
||||
# ------------------------------------------------------------------ 公開方法
|
||||
|
||||
def start(self, planned_waypoints):
|
||||
"""啟動任務"""
|
||||
if self.state == MissionState.RUNNING:
|
||||
print("任務已在執行中")
|
||||
return
|
||||
@@ -93,6 +116,9 @@ class MissionExecutor(QObject):
|
||||
|
||||
drone_ids = planned_waypoints['drone_ids']
|
||||
waypoints_list = planned_waypoints['waypoints']
|
||||
self.rendezvous_indices = set(
|
||||
planned_waypoints.get('rendezvous_indices', []) or []
|
||||
)
|
||||
|
||||
for i, drone_id in enumerate(drone_ids):
|
||||
sysid = int(drone_id.split('_')[1])
|
||||
@@ -102,58 +128,121 @@ class MissionExecutor(QObject):
|
||||
self._timer.start(self._interval_ms)
|
||||
|
||||
total_wps = sum(t.total_waypoints for t in self.tasks.values())
|
||||
rv_info = (
|
||||
f"rendezvous WP={sorted(self.rendezvous_indices)}"
|
||||
if self.rendezvous_indices else "無 rendezvous (各自跑)"
|
||||
)
|
||||
print(f"任務啟動: {len(self.tasks)} 架無人機, "
|
||||
f"共 {total_wps} 個航點, "
|
||||
f"到達半徑={self.arrival_radius}m, "
|
||||
f"發送週期={self._interval_ms}ms")
|
||||
f"tick 週期={self._interval_ms}ms, "
|
||||
f"barrier timeout={self.barrier_timeout_sec}s, "
|
||||
f"{rv_info}")
|
||||
|
||||
def pause(self):
|
||||
"""暫停任務"""
|
||||
if self.state == MissionState.RUNNING:
|
||||
self._timer.stop()
|
||||
self.state = MissionState.PAUSED
|
||||
print("任務暫停")
|
||||
|
||||
def resume(self):
|
||||
"""恢復任務"""
|
||||
if self.state == MissionState.PAUSED:
|
||||
self._timer.start(self._interval_ms)
|
||||
self.state = MissionState.RUNNING
|
||||
print("任務恢復")
|
||||
|
||||
def stop(self):
|
||||
"""停止任務"""
|
||||
self._timer.stop()
|
||||
self.tasks.clear()
|
||||
self.rendezvous_indices = set()
|
||||
self.state = MissionState.IDLE
|
||||
print("任務停止")
|
||||
|
||||
# ------------------------------------------------------------------ 控制迴圈
|
||||
|
||||
def _tick(self):
|
||||
"""控制迴圈"""
|
||||
all_done = True
|
||||
"""
|
||||
控制 tick：
|
||||
Phase 1: 每台機做到達判定；非 barrier 點直接推進，barrier 點改成 WAITING
|
||||
Phase 2: 檢查 barrier 能不能釋放（全員到齊或 timeout）
|
||||
Phase 3: 每台未送過當前航點的機送一次
|
||||
Phase 4: 全部完成檢查
|
||||
"""
|
||||
now = time.monotonic()
|
||||
|
||||
# ---- Phase 1: 到達判定 ----
|
||||
for task in self.tasks.values():
|
||||
if task.done:
|
||||
if task.done or task.status == TaskStatus.FALLBACK_LOITER:
|
||||
continue
|
||||
# 已在 barrier 等待的不再跑到達判定
|
||||
if task.status == TaskStatus.WAITING_AT_BARRIER:
|
||||
continue
|
||||
|
||||
all_done = False
|
||||
target = task.current_target
|
||||
if target is None:
|
||||
continue
|
||||
|
||||
# 讀取當前 GPS
|
||||
gps = self.drone_gps.get(task.drone_id)
|
||||
if gps is None:
|
||||
continue
|
||||
|
||||
tgt_lat, tgt_lon, tgt_alt = target
|
||||
tgt_lat, tgt_lon, _ = target
|
||||
distance = _haversine(gps['lat'], gps['lon'], tgt_lat, tgt_lon)
|
||||
if distance >= self.arrival_radius:
|
||||
continue
|
||||
|
||||
# 到達判定
|
||||
if distance < self.arrival_radius:
|
||||
# 到達當前 wp_index
|
||||
if (task.wp_index in self.rendezvous_indices
|
||||
and task.wp_index < task.total_waypoints - 1):
|
||||
# rendezvous 點 → 不推進，進入 barrier 等待
|
||||
task.status = TaskStatus.WAITING_AT_BARRIER
|
||||
task.waiting_since = now
|
||||
print(f" {task.drone_id} → 抵達 barrier WP {task.wp_index}，等待同伴")
|
||||
self.task_status_changed.emit(
|
||||
task.drone_id, task.status.value,
|
||||
f"waiting at WP {task.wp_index}"
|
||||
)
|
||||
continue
|
||||
|
||||
# 一般點 → 直接推進
|
||||
self._advance_waypoint(task, distance)
|
||||
|
||||
# ---- Phase 2: barrier 釋放檢查 ----
|
||||
self._check_barriers(now)
|
||||
|
||||
# ---- Phase 3: 發送未送過的目標 ----
|
||||
for task in self.tasks.values():
|
||||
if task.done or task.status in (
|
||||
TaskStatus.FALLBACK_LOITER, TaskStatus.WAITING_AT_BARRIER
|
||||
):
|
||||
continue
|
||||
if task.sent_current_wp:
|
||||
continue
|
||||
target = task.current_target
|
||||
if target is None:
|
||||
continue
|
||||
tgt_lat, tgt_lon, tgt_alt = target
|
||||
task.sent_current_wp = True
|
||||
self.sender.send_position_global(
|
||||
task.drone_id, task.sysid, tgt_lat, tgt_lon, tgt_alt
|
||||
)
|
||||
|
||||
# ---- Phase 4: 完成檢查 ----
|
||||
all_done = all(
|
||||
t.done or t.status == TaskStatus.FALLBACK_LOITER
|
||||
for t in self.tasks.values()
|
||||
)
|
||||
if all_done and self.tasks:
|
||||
self._timer.stop()
|
||||
self.state = MissionState.IDLE
|
||||
self.mission_completed.emit()
|
||||
print("===== 任務全部完成 =====")
|
||||
|
||||
def _advance_waypoint(self, task, arrived_distance):
|
||||
"""把 task 推進一個航點，重置發送旗標。不處理 barrier 邏輯。"""
|
||||
task.wp_index += 1
|
||||
task.sent_current_wp = False
|
||||
task.fail_count = 0
|
||||
if task.wp_index >= task.total_waypoints:
|
||||
task.done = True
|
||||
self.drone_waypoint_reached.emit(
|
||||
@@ -161,27 +250,109 @@ class MissionExecutor(QObject):
|
||||
)
|
||||
print(f" {task.drone_id} → DONE "
|
||||
f"({task.total_waypoints}/{task.total_waypoints})")
|
||||
continue
|
||||
else:
|
||||
return
|
||||
self.drone_waypoint_reached.emit(
|
||||
task.drone_id, task.wp_index, task.total_waypoints
|
||||
)
|
||||
print(f" {task.drone_id} → WP {task.wp_index}/{task.total_waypoints} "
|
||||
f"(距離: {distance:.1f}m)")
|
||||
# 更新目標
|
||||
tgt_lat, tgt_lon, tgt_alt = task.current_target
|
||||
f"(到達距離: {arrived_distance:.1f}m)")
|
||||
|
||||
# 發送 setpoint
|
||||
self.sender.send_position_global(
|
||||
task.sysid, tgt_lat, tgt_lon, tgt_alt
|
||||
def _check_barriers(self, now):
|
||||
"""檢查每個有人在等的 barrier 能不能釋放。"""
|
||||
# 蒐集目前有哪些 barrier 有人卡著
|
||||
waiting_groups = {} # barrier_idx -> [task, ...]
|
||||
for task in self.tasks.values():
|
||||
if task.status == TaskStatus.WAITING_AT_BARRIER:
|
||||
waiting_groups.setdefault(task.wp_index, []).append(task)
|
||||
|
||||
if not waiting_groups:
|
||||
return
|
||||
|
||||
for barrier_idx, waiting_tasks in waiting_groups.items():
|
||||
# 判斷是否「全員到齊」：所有仍活著的機都要麼在此 barrier 等，要麼早已越過此 idx
|
||||
everyone_arrived = True
|
||||
for task in self.tasks.values():
|
||||
if task.done or task.status == TaskStatus.FALLBACK_LOITER:
|
||||
continue # 失敗機/完成機不擋
|
||||
if (task.status == TaskStatus.WAITING_AT_BARRIER
|
||||
and task.wp_index == barrier_idx):
|
||||
continue # 在這裡等
|
||||
if task.wp_index > barrier_idx:
|
||||
continue # 早已越過（barrier 先前已對他釋放）
|
||||
# 還在路上
|
||||
everyone_arrived = False
|
||||
break
|
||||
|
||||
force_reason = None
|
||||
if not everyone_arrived:
|
||||
# 檢查 timeout：以該 barrier 上最早進入等待的時間為準
|
||||
oldest_wait = min(t.waiting_since for t in waiting_tasks)
|
||||
if now - oldest_wait >= self.barrier_timeout_sec:
|
||||
force_reason = f"timeout {self.barrier_timeout_sec:.0f}s"
|
||||
print(f"⚠️ barrier WP {barrier_idx} {force_reason}，強制放行")
|
||||
else:
|
||||
continue # 還沒到齊、也還沒 timeout → 繼續等
|
||||
|
||||
# 釋放：把所有在此 barrier 等待的機一起推進
|
||||
tag = "全員到齊" if force_reason is None else force_reason
|
||||
print(f"✅ barrier WP {barrier_idx} 釋放 ({tag})，"
|
||||
f"推進 {len(waiting_tasks)} 架")
|
||||
for task in waiting_tasks:
|
||||
task.status = TaskStatus.NORMAL
|
||||
msg = f"barrier WP {barrier_idx} released ({tag})"
|
||||
self.task_status_changed.emit(
|
||||
task.drone_id, task.status.value, msg
|
||||
)
|
||||
self._advance_waypoint(task, self.arrival_radius)
|
||||
|
||||
# 全部完成檢查
|
||||
if all_done and self.tasks:
|
||||
self._timer.stop()
|
||||
self.state = MissionState.IDLE
|
||||
self.mission_completed.emit()
|
||||
print("===== 任務全部完成 =====")
|
||||
# ------------------------------------------------------------------ 結果回呼
|
||||
|
||||
def _on_send_result(self, drone_id, sysid, success, message):
|
||||
"""Ros2CommandSender.send_result 的 slot"""
|
||||
task = self.tasks.get(drone_id)
|
||||
if task is None or task.done:
|
||||
return
|
||||
# 若已在 barrier 等待，舊指令的遲到回應不要再觸發重試/fallback
|
||||
if task.status == TaskStatus.WAITING_AT_BARRIER:
|
||||
return
|
||||
|
||||
if success:
|
||||
if task.fail_count > 0 or task.status == TaskStatus.RETRYING:
|
||||
task.status = TaskStatus.NORMAL
|
||||
self.task_status_changed.emit(drone_id, task.status.value, "recovered")
|
||||
task.fail_count = 0
|
||||
return
|
||||
|
||||
task.fail_count += 1
|
||||
print(f"⚠️ {drone_id} 發送失敗 {task.fail_count}/{self.MAX_RETRY}: {message}")
|
||||
|
||||
if task.fail_count < self.MAX_RETRY:
|
||||
task.status = TaskStatus.RETRYING
|
||||
task.sent_current_wp = False # 下個 tick 會重送
|
||||
self.task_status_changed.emit(
|
||||
drone_id, task.status.value,
|
||||
f"retry {task.fail_count}/{self.MAX_RETRY}: {message}"
|
||||
)
|
||||
else:
|
||||
task.status = TaskStatus.FALLBACK_LOITER
|
||||
self.task_status_changed.emit(
|
||||
drone_id, task.status.value,
|
||||
f"fallback LOITER after {self.MAX_RETRY} fails: {message}"
|
||||
)
|
||||
print(f"❌ {drone_id} 連續失敗 {self.MAX_RETRY} 次 → 切換 LOITER")
|
||||
self._fallback_to_loiter(drone_id)
|
||||
|
||||
def _fallback_to_loiter(self, drone_id):
|
||||
"""用 monitor.set_mode 切 LOITER。set_mode 是 coroutine，透過 event loop 派送。"""
|
||||
if self.monitor is None:
|
||||
print(f"⚠️ 無 monitor，無法將 {drone_id} 切到 LOITER")
|
||||
return
|
||||
try:
|
||||
loop = asyncio.get_event_loop()
|
||||
except RuntimeError:
|
||||
loop = asyncio.new_event_loop()
|
||||
asyncio.set_event_loop(loop)
|
||||
loop.create_task(self.monitor.set_mode(drone_id, "LOITER"))
|
||||
|
||||
|
||||
# ------------------------------------------------------------------ 工具函式
|
||||
|
||||
@@ -35,6 +35,7 @@ class MissionGroup:
|
||||
self.planned_waypoints = None # 規劃結果 dict
|
||||
self.executor = None # MissionExecutor 實例（延遲建立）
|
||||
self.center_origin = None # 規劃原點
|
||||
self.leader_drone_id = None # LEADER_FOLLOWER 專用：指定的領隊無人機 ID
|
||||
|
||||
@property
|
||||
def state(self):
|
||||
@@ -415,6 +416,21 @@ class GroupPanel(QWidget):
|
||||
self._param_widgets[key] = (row_w, inp)
|
||||
self._param_rows.append(row_w)
|
||||
|
||||
# LEADER_FOLLOWER 專用：領隊下拉選單
|
||||
self._leader_row = QWidget()
|
||||
leader_layout = QHBoxLayout(self._leader_row)
|
||||
leader_layout.setContentsMargins(0, 0, 0, 0)
|
||||
leader_layout.setSpacing(3)
|
||||
leader_lbl = QLabel("領隊")
|
||||
leader_lbl.setStyleSheet(LBL)
|
||||
self._leader_combo = QComboBox()
|
||||
self._leader_combo.setStyleSheet(COMBO)
|
||||
self._leader_combo.setFixedWidth(70)
|
||||
self._leader_combo.currentTextChanged.connect(self._on_leader_changed)
|
||||
leader_layout.addWidget(leader_lbl, 1)
|
||||
leader_layout.addWidget(self._leader_combo)
|
||||
param_col.addWidget(self._leader_row)
|
||||
|
||||
param_col.addStretch()
|
||||
|
||||
# 初始顯示對應的參數
|
||||
@@ -447,6 +463,27 @@ class GroupPanel(QWidget):
|
||||
self.drone_list_label.setText("、".join(sorted_ids))
|
||||
self.drone_list_label.setStyleSheet(
|
||||
f"color: {self.group.color}; font-size: 11px; font-weight: bold;")
|
||||
self._refresh_leader_options()
|
||||
|
||||
def _refresh_leader_options(self):
|
||||
"""依目前群組成員重新填充領隊下拉選單，保留目前選擇若仍有效"""
|
||||
sorted_ids = sorted(self.group.drone_ids,
|
||||
key=lambda x: (x.split('_')[0], int(x.split('_')[1])))
|
||||
current = self.group.leader_drone_id
|
||||
self._leader_combo.blockSignals(True)
|
||||
self._leader_combo.clear()
|
||||
self._leader_combo.addItems(sorted_ids)
|
||||
if current and current in sorted_ids:
|
||||
self._leader_combo.setCurrentText(current)
|
||||
elif sorted_ids:
|
||||
self._leader_combo.setCurrentText(sorted_ids[0])
|
||||
self.group.leader_drone_id = sorted_ids[0]
|
||||
else:
|
||||
self.group.leader_drone_id = None
|
||||
self._leader_combo.blockSignals(False)
|
||||
|
||||
def _on_leader_changed(self, drone_id):
|
||||
self.group.leader_drone_id = drone_id if drone_id else None
|
||||
|
||||
def update_status(self):
|
||||
"""更新任務狀態顯示"""
|
||||
@@ -485,6 +522,7 @@ class GroupPanel(QWidget):
|
||||
visible_keys = {d[0] for d in self._param_defs.get(mission_type, [])}
|
||||
for key, (row_w, _inp) in self._param_widgets.items():
|
||||
row_w.setVisible(key in visible_keys)
|
||||
self._leader_row.setVisible(mission_type == 'LEADER_FOLLOWER')
|
||||
|
||||
def get_mission_params(self):
|
||||
"""讀取當前顯示的參數值，回傳 dict"""
|
||||
|
||||
+285
-120
@@ -1,7 +1,7 @@
|
||||
#!/usr/bin/env python3
|
||||
"""
|
||||
飛行任務規劃模組
|
||||
支援: M-Formation, Circle, Leader-Follower (Bezier 轉彎), Grid Sweep
|
||||
支援: M-Formation, Circle, Leader-Follower (arc-length virtual leader), Grid Sweep
|
||||
"""
|
||||
import math
|
||||
from typing import List, Tuple, Optional, Dict, Any
|
||||
@@ -72,7 +72,12 @@ class FormationPlanner:
|
||||
mission_type: MissionType = MissionType.M_FORMATION,
|
||||
params: Optional[Dict[str, Any]] = None
|
||||
) -> Tuple[List[List[Tuple[float, float, float]]],
|
||||
Tuple[float, float, float]]:
|
||||
Tuple[float, float, float],
|
||||
List[int]]:
|
||||
"""
|
||||
回傳 (waypoints_gps, origin, rendezvous_indices)。
|
||||
rendezvous_indices：航點序列裡該群組需要等全員到齊才推進的 index 清單。
|
||||
"""
|
||||
if len(drone_gps_positions) == 0:
|
||||
raise ValueError("無人機位置列表不能為空")
|
||||
|
||||
@@ -89,10 +94,14 @@ class FormationPlanner:
|
||||
if mission_type == MissionType.M_FORMATION:
|
||||
s1, s2 = self._calculate_m_formation(drone_local, target_local, params)
|
||||
waypoints_local = [[s1[i], s2[i]] for i in range(len(drone_local))]
|
||||
# 起點集合後一起出發到 stage2
|
||||
rendezvous_indices = [0]
|
||||
|
||||
elif mission_type == MissionType.CIRCLE_FORMATION:
|
||||
s1, s2 = self._calculate_circle_formation(drone_local, target_local, params)
|
||||
waypoints_local = [[s1[i], s2[i]] for i in range(len(drone_local))]
|
||||
# 半程集合後一起進最終圓環位置
|
||||
rendezvous_indices = [0]
|
||||
|
||||
elif mission_type == MissionType.LEADER_FOLLOWER:
|
||||
params = params or {}
|
||||
@@ -103,7 +112,9 @@ class FormationPlanner:
|
||||
self.converter.gps_to_local(wp[0], wp[1], 0)[:2]
|
||||
for wp in route_wps_gps
|
||||
]
|
||||
waypoints_local = self._calculate_leader_follower(drone_local, route_wps_local, params)
|
||||
waypoints_local, rendezvous_indices = self._calculate_leader_follower(
|
||||
drone_local, route_wps_local, params
|
||||
)
|
||||
|
||||
elif mission_type == MissionType.GRID_SWEEP:
|
||||
params = params or {}
|
||||
@@ -114,7 +125,9 @@ class FormationPlanner:
|
||||
self.converter.gps_to_local(c[0], c[1], 0)[:2]
|
||||
for c in rect_corners_gps
|
||||
]
|
||||
waypoints_local = self._calculate_grid_sweep(drone_local, rect_corners_local, params)
|
||||
waypoints_local, rendezvous_indices = self._calculate_grid_sweep(
|
||||
drone_local, rect_corners_local, params
|
||||
)
|
||||
else:
|
||||
raise ValueError(f"不支援的任務類型: {mission_type}")
|
||||
|
||||
@@ -123,7 +136,7 @@ class FormationPlanner:
|
||||
gps_wps = [self.converter.local_to_gps(*wp) for wp in drone_wps]
|
||||
waypoints_gps.append(gps_wps)
|
||||
|
||||
return waypoints_gps, self.current_origin
|
||||
return waypoints_gps, self.current_origin, rendezvous_indices
|
||||
|
||||
# ------------------------------------------------------------------ M-Formation
|
||||
|
||||
@@ -189,177 +202,321 @@ class FormationPlanner:
|
||||
|
||||
return stage1_positions, stage2_positions
|
||||
|
||||
# ------------------------------------------------------------------ 路徑跟隨 (Bezier 轉彎)
|
||||
# ------------------------------------------------------------------ 路徑跟隨 (虛擬領隊)
|
||||
|
||||
def _calculate_leader_follower(self, drone_positions, route_wps_local, params):
|
||||
"""
|
||||
路徑跟隨編隊 — Bezier 曲線轉彎版
|
||||
路徑跟隨編隊 — 虛擬領隊 / 弧長參數化版 (virtual leader / arc-length)
|
||||
|
||||
步驟:
|
||||
1. _build_center_path: 在轉折 WP 處用二次 Bezier 曲線平滑轉彎
|
||||
2. 固定排序，每架無人機沿中心路徑套用橫向+縱向偏移
|
||||
核心想法：
|
||||
- center_path 建好後計算每個 sample 的累積弧長 s
|
||||
- 每架 drone 給 (lat_k, lon_k) 的隊形偏移
|
||||
- 把 leader 想成沿 s 參數化的點，follower k 的目標 s = s_leader - lon_k
|
||||
- follower k 第 i 個 waypoint = lookup(s_list[i] - lon_k) + lat_k × perp(tangent)
|
||||
- 超過 path 起點/終點以 clip 處理，避免倒退或衝出
|
||||
|
||||
隊形 (俯視):
|
||||
| (前進方向) |
|
||||
| D1 | ← 左偏移, 後 0m
|
||||
| D2 | ← 右偏移, 後 5m
|
||||
| D3 | ← 左偏移, 後 10m
|
||||
| D4 | ← 右偏移, 後 15m
|
||||
解決前版「空間偏移」的三個 bug：
|
||||
1. 起點暴衝：s<0 clip → follower 起點就是 start 加橫向偏移，不往後倒退
|
||||
2. 弧段角度爆炸：lon 不再換算成弧度，直接沿 polyline lookup
|
||||
3. straight/arc 切換不連續：統一走 polyline 線性內插與 segment 切線
|
||||
|
||||
Rank 定序規則：
|
||||
**以 drone_positions 的輸入順序為準**（= GUI 選取順序）。
|
||||
drone_positions[0] = rank 0 = leader (lon=0)，
|
||||
drone_positions[1] = rank 1 (lon=5)，依此類推。
|
||||
刻意「不」用位置投影自動排序，避免浮點噪音造成 run-to-run
|
||||
leader 漂移，以確保整個 mission 像軍隊縱隊那樣順序固定。
|
||||
|
||||
隊形 (俯視, 以路徑行進方向為前):
|
||||
D0 (lat=-L, lon=0) ← front-right (leader)
|
||||
D1 (lat=+L, lon=5)
|
||||
D2 (lat=-L, lon=10)
|
||||
D3 (lat=+L, lon=15)
|
||||
"""
|
||||
N = len(drone_positions)
|
||||
lateral_offset = params.get('lateral_offset', 3.0)
|
||||
longitudinal_spacing = params.get('longitudinal_spacing', 5.0)
|
||||
altitude = params.get('altitude', self.base_altitude)
|
||||
turn_margin = params.get('turn_margin', 0.35) # 轉彎切入距離佔段長比例
|
||||
curve_resolution = params.get('curve_resolution', 8) # 每個彎道的插值點數
|
||||
min_inner_radius = params.get('min_inner_radius', 3.0)
|
||||
arc_resolution = params.get('arc_resolution', 12)
|
||||
sharp_turn_cos = params.get('sharp_turn_cos_threshold', 0.0) # cos(90°)=0
|
||||
|
||||
# Step 1: 建立帶 Bezier 轉彎的中心路徑
|
||||
center_path = self._build_center_path(
|
||||
route_wps_local, turn_margin, curve_resolution
|
||||
# Step 1: 建立中心路徑（含圓弧、銳角單點；每點帶累積 s）
|
||||
center_path, barrier_indices = self._build_center_path(
|
||||
route_wps_local,
|
||||
formation_max_lateral=lateral_offset,
|
||||
min_inner_radius=min_inner_radius,
|
||||
arc_resolution=arc_resolution,
|
||||
sharp_turn_cos_threshold=sharp_turn_cos,
|
||||
)
|
||||
|
||||
# Step 2: 固定排序
|
||||
first_dir = (center_path[0][2], center_path[0][3])
|
||||
first_perp = (-first_dir[1], first_dir[0])
|
||||
C_x = sum(p[0] for p in drone_positions) / N
|
||||
C_y = sum(p[1] for p in drone_positions) / N
|
||||
s_list = [pt['s'] for pt in center_path]
|
||||
s_max = s_list[-1]
|
||||
n_pts = len(center_path)
|
||||
|
||||
projections = [
|
||||
((pos[0] - C_x) * first_perp[0] + (pos[1] - C_y) * first_perp[1], i)
|
||||
for i, pos in enumerate(drone_positions)
|
||||
]
|
||||
projections.sort()
|
||||
def lookup(s_target):
|
||||
"""
|
||||
在 center path 上以弧長 s 回傳 (x, y, tangent_dir)。
|
||||
s<0 → clip 到 start；s>s_max → clip 到 end。
|
||||
tangent 方向取當前 polyline segment 的切線，避免對不連續的 dir 做插值。
|
||||
"""
|
||||
if n_pts == 1:
|
||||
pt = center_path[0]
|
||||
return pt['x'], pt['y'], pt['dir']
|
||||
|
||||
# Step 3: 偏移
|
||||
if s_target <= 0.0:
|
||||
a = center_path[0]
|
||||
b = center_path[1]
|
||||
sdx, sdy = b['x'] - a['x'], b['y'] - a['y']
|
||||
slen = math.hypot(sdx, sdy)
|
||||
if slen > 1e-6:
|
||||
return a['x'], a['y'], (sdx / slen, sdy / slen)
|
||||
return a['x'], a['y'], a['dir']
|
||||
|
||||
if s_target >= s_max:
|
||||
a = center_path[-2]
|
||||
b = center_path[-1]
|
||||
sdx, sdy = b['x'] - a['x'], b['y'] - a['y']
|
||||
slen = math.hypot(sdx, sdy)
|
||||
if slen > 1e-6:
|
||||
return b['x'], b['y'], (sdx / slen, sdy / slen)
|
||||
return b['x'], b['y'], b['dir']
|
||||
|
||||
# Binary search：s_list[lo] <= s_target < s_list[hi]
|
||||
lo, hi = 0, n_pts - 1
|
||||
while lo + 1 < hi:
|
||||
mid = (lo + hi) // 2
|
||||
if s_list[mid] <= s_target:
|
||||
lo = mid
|
||||
else:
|
||||
hi = mid
|
||||
a = center_path[lo]
|
||||
b = center_path[hi]
|
||||
ds = s_list[hi] - s_list[lo]
|
||||
if ds < 1e-9:
|
||||
return a['x'], a['y'], a['dir']
|
||||
t = (s_target - s_list[lo]) / ds
|
||||
x = a['x'] + t * (b['x'] - a['x'])
|
||||
y = a['y'] + t * (b['y'] - a['y'])
|
||||
sdx, sdy = b['x'] - a['x'], b['y'] - a['y']
|
||||
slen = math.hypot(sdx, sdy)
|
||||
if slen > 1e-6:
|
||||
return x, y, (sdx / slen, sdy / slen)
|
||||
return x, y, a['dir']
|
||||
|
||||
# Step 2: rank = 輸入順序（GUI 選取順序），不再做 projection sort
|
||||
# 避免浮點噪音在投影值相近時翻轉 leader，保證 run-to-run 穩定
|
||||
all_waypoints = [None] * N
|
||||
|
||||
for rank, (_, original_idx) in enumerate(projections):
|
||||
# 預先算好路徑終點的切線（給收尾點用）
|
||||
end = center_path[-1]
|
||||
end_dx, end_dy = end['dir']
|
||||
if n_pts >= 2:
|
||||
a = center_path[-2]
|
||||
sdx, sdy = end['x'] - a['x'], end['y'] - a['y']
|
||||
slen = math.hypot(sdx, sdy)
|
||||
if slen > 1e-6:
|
||||
end_dx, end_dy = sdx / slen, sdy / slen
|
||||
|
||||
for rank in range(N):
|
||||
lat_sign = -1 if rank % 2 == 0 else 1
|
||||
lat = lat_sign * lateral_offset
|
||||
lon = rank * longitudinal_spacing
|
||||
|
||||
waypoints = []
|
||||
for (cx, cy, dx, dy) in center_path:
|
||||
for i in range(n_pts):
|
||||
s_follower = s_list[i] - lon
|
||||
x, y, (dx, dy) = lookup(s_follower)
|
||||
perp_x, perp_y = -dy, dx
|
||||
ox = cx + lat * perp_x - lon * dx
|
||||
oy = cy + lat * perp_y - lon * dy
|
||||
waypoints.append((ox, oy, altitude))
|
||||
waypoints.append((
|
||||
x + lat * perp_x,
|
||||
y + lat * perp_y,
|
||||
altitude,
|
||||
))
|
||||
|
||||
all_waypoints[original_idx] = waypoints
|
||||
# 收尾：全員到 path 終點的 rank 位置（lon 歸零）
|
||||
waypoints.append((
|
||||
end['x'] + lat * (-end_dy),
|
||||
end['y'] + lat * end_dx,
|
||||
altitude,
|
||||
))
|
||||
|
||||
return all_waypoints
|
||||
all_waypoints[rank] = waypoints
|
||||
|
||||
def _build_center_path(self, waypoints, turn_margin, curve_resolution):
|
||||
# barrier 索引仍指向 center_path 內 (range [0, n_pts-1])，
|
||||
# 不觸及末尾收尾點，直接沿用即可
|
||||
return all_waypoints, barrier_indices
|
||||
|
||||
def _build_center_path(self, waypoints,
|
||||
formation_max_lateral,
|
||||
min_inner_radius,
|
||||
arc_resolution,
|
||||
sharp_turn_cos_threshold):
|
||||
"""
|
||||
建立帶 Bezier 曲線轉彎的中心路徑
|
||||
建立以「圓弧」連接直線段的中心路徑。
|
||||
|
||||
在每個轉折 WP 處:
|
||||
1. 計算 pre_turn = WP - d_in × T
|
||||
2. 計算 post_turn = WP + d_out × T
|
||||
3. 用二次 Bezier 曲線: B(t) = (1-t)²·pre + 2t(1-t)·WP + t²·post
|
||||
4. 方向從導數得到: B'(t) = 2(1-t)(WP-pre) + 2t(post-WP)
|
||||
每個中間 waypoint 的處理：
|
||||
1. 計算入向 u_in、出向 u_out 的夾角 β = acos(u_in·u_out)
|
||||
2. 若 cos β < sharp_turn_cos_threshold → 銳角：只插單點 (hover + 原地轉向)，
|
||||
barrier 放在該點，讓隊伍整體停下再繼續
|
||||
3. 否則 → 平滑弧：
|
||||
R_base = formation_max_lateral + min_inner_radius
|
||||
d = R_base × tan(β/2)，並以相鄰段長的 45% 為上限
|
||||
R_actual = d / tan(β/2)
|
||||
tangent points T_in = WP - u_in·d, T_out = WP + u_out·d
|
||||
arc center = T_in + R_actual·sign·n_left，其中 sign=+1 左轉/CCW，-1 右轉/CW
|
||||
arc 由 theta_in 到 theta_out 以 arc_resolution 等分
|
||||
path 內容: [T_in(straight,u_in), arc_samples..., T_out(straight,u_out)]
|
||||
barrier 放在 T_out (轉完彎後的集合點)
|
||||
|
||||
Args:
|
||||
waypoints: 路徑航點 [(x, y), ...]
|
||||
turn_margin: 轉彎切入距離佔相鄰段長的比例 (0~0.5)
|
||||
curve_resolution: 每個彎道的 Bezier 插值點數
|
||||
path 是一個 list[dict]，每個 dict 至少含:
|
||||
{'seg': 'straight' | 'arc',
|
||||
'x': float, 'y': float,
|
||||
'dir': (dx, dy)}
|
||||
'arc' 類型額外含: 'arc_center', 'arc_R', 'arc_sign', 'theta'
|
||||
|
||||
Returns:
|
||||
[(x, y, dir_x, dir_y), ...] 中心路徑點 + 單位方向
|
||||
(path, barrier_indices)
|
||||
"""
|
||||
num_wps = len(waypoints)
|
||||
|
||||
if num_wps < 2:
|
||||
return [(waypoints[0][0], waypoints[0][1], 0.0, 1.0)]
|
||||
return [{
|
||||
'seg': 'straight',
|
||||
'x': waypoints[0][0], 'y': waypoints[0][1],
|
||||
'dir': (0.0, 1.0),
|
||||
}], []
|
||||
|
||||
# 計算每段方向和長度
|
||||
segments = []
|
||||
for i in range(num_wps - 1):
|
||||
dx = waypoints[i + 1][0] - waypoints[i][0]
|
||||
dy = waypoints[i + 1][1] - waypoints[i][1]
|
||||
length = math.sqrt(dx ** 2 + dy ** 2)
|
||||
length = math.hypot(dx, dy)
|
||||
if length < 0.01:
|
||||
segments.append((0.0, 1.0, length))
|
||||
segments.append(((0.0, 1.0), length))
|
||||
else:
|
||||
segments.append((dx / length, dy / length, length))
|
||||
segments.append(((dx / length, dy / length), length))
|
||||
|
||||
R_base = formation_max_lateral + min_inner_radius
|
||||
path = []
|
||||
barrier_indices = []
|
||||
|
||||
# 第一個航點
|
||||
path.append((waypoints[0][0], waypoints[0][1],
|
||||
segments[0][0], segments[0][1]))
|
||||
# 起點
|
||||
path.append({
|
||||
'seg': 'straight',
|
||||
'x': waypoints[0][0], 'y': waypoints[0][1],
|
||||
'dir': segments[0][0],
|
||||
})
|
||||
|
||||
# 中間航點 (轉彎處)
|
||||
for i in range(1, num_wps - 1):
|
||||
d_in_x, d_in_y, len_in = segments[i - 1]
|
||||
d_out_x, d_out_y, len_out = segments[i]
|
||||
u_in, len_in = segments[i - 1]
|
||||
u_out, len_out = segments[i]
|
||||
wp = waypoints[i]
|
||||
|
||||
# 切入距離 T: 取相鄰兩段中較短的 × turn_margin
|
||||
T = min(len_in, len_out) * turn_margin
|
||||
dot = u_in[0] * u_out[0] + u_in[1] * u_out[1]
|
||||
dot = max(-1.0, min(1.0, dot))
|
||||
|
||||
if T < 0.5:
|
||||
# 段太短，不插彎，直接加一個平均方向點
|
||||
avg_dx = d_in_x + d_out_x
|
||||
avg_dy = d_in_y + d_out_y
|
||||
avg_len = math.sqrt(avg_dx ** 2 + avg_dy ** 2)
|
||||
# 銳角：hover + 原地轉向
|
||||
if dot < sharp_turn_cos_threshold:
|
||||
avg_dx = u_in[0] + u_out[0]
|
||||
avg_dy = u_in[1] + u_out[1]
|
||||
avg_len = math.hypot(avg_dx, avg_dy)
|
||||
if avg_len > 0.01:
|
||||
avg_dx /= avg_len
|
||||
avg_dy /= avg_len
|
||||
avg_dir = (avg_dx / avg_len, avg_dy / avg_len)
|
||||
else:
|
||||
avg_dx, avg_dy = d_in_x, d_in_y
|
||||
path.append((waypoints[i][0], waypoints[i][1], avg_dx, avg_dy))
|
||||
avg_dir = u_in
|
||||
path.append({
|
||||
'seg': 'straight',
|
||||
'x': wp[0], 'y': wp[1],
|
||||
'dir': avg_dir,
|
||||
})
|
||||
barrier_indices.append(len(path) - 1)
|
||||
continue
|
||||
|
||||
# P0 = pre_turn (弧線起始)
|
||||
p0_x = waypoints[i][0] - d_in_x * T
|
||||
p0_y = waypoints[i][1] - d_in_y * T
|
||||
# 平滑弧
|
||||
beta = math.acos(dot)
|
||||
if beta < 1e-4:
|
||||
# 幾乎直線，不插弧
|
||||
path.append({
|
||||
'seg': 'straight',
|
||||
'x': wp[0], 'y': wp[1],
|
||||
'dir': u_in,
|
||||
})
|
||||
continue
|
||||
|
||||
# P1 = WP 本身 (控制點)
|
||||
p1_x = waypoints[i][0]
|
||||
p1_y = waypoints[i][1]
|
||||
half_tan = math.tan(beta / 2.0)
|
||||
d_ideal = R_base * half_tan
|
||||
d_max = 0.45 * min(len_in, len_out)
|
||||
d = min(d_ideal, d_max)
|
||||
R_actual = d / half_tan
|
||||
|
||||
# P2 = post_turn (弧線結束)
|
||||
p2_x = waypoints[i][0] + d_out_x * T
|
||||
p2_y = waypoints[i][1] + d_out_y * T
|
||||
T_in = (wp[0] - u_in[0] * d, wp[1] - u_in[1] * d)
|
||||
T_out = (wp[0] + u_out[0] * d, wp[1] + u_out[1] * d)
|
||||
|
||||
# 加入 pre_turn 點 (方向 = incoming)
|
||||
path.append((p0_x, p0_y, d_in_x, d_in_y))
|
||||
# +1 CCW (左轉) / -1 CW (右轉)
|
||||
cross = u_in[0] * u_out[1] - u_in[1] * u_out[0]
|
||||
sign = 1 if cross > 0 else -1
|
||||
|
||||
# 加入 Bezier 插值點
|
||||
for j in range(1, curve_resolution):
|
||||
t = j / curve_resolution
|
||||
# 入向左法線
|
||||
n_left = (-u_in[1], u_in[0])
|
||||
center = (
|
||||
T_in[0] + R_actual * sign * n_left[0],
|
||||
T_in[1] + R_actual * sign * n_left[1],
|
||||
)
|
||||
|
||||
# B(t) = (1-t)²·P0 + 2t(1-t)·P1 + t²·P2
|
||||
one_minus_t = 1.0 - t
|
||||
bx = one_minus_t * one_minus_t * p0_x + \
|
||||
2 * t * one_minus_t * p1_x + \
|
||||
t * t * p2_x
|
||||
by = one_minus_t * one_minus_t * p0_y + \
|
||||
2 * t * one_minus_t * p1_y + \
|
||||
t * t * p2_y
|
||||
theta_in = math.atan2(T_in[1] - center[1], T_in[0] - center[0])
|
||||
|
||||
# B'(t) = 2(1-t)(P1-P0) + 2t(P2-P1) → 切線方向
|
||||
tdx = 2 * one_minus_t * (p1_x - p0_x) + 2 * t * (p2_x - p1_x)
|
||||
tdy = 2 * one_minus_t * (p1_y - p0_y) + 2 * t * (p2_y - p1_y)
|
||||
# T_in (straight, 方向 u_in)
|
||||
path.append({
|
||||
'seg': 'straight',
|
||||
'x': T_in[0], 'y': T_in[1],
|
||||
'dir': u_in,
|
||||
})
|
||||
|
||||
# 正規化
|
||||
t_len = math.sqrt(tdx ** 2 + tdy ** 2)
|
||||
if t_len > 0.01:
|
||||
tdx /= t_len
|
||||
tdy /= t_len
|
||||
else:
|
||||
tdx, tdy = d_in_x, d_in_y
|
||||
# 弧段採樣 k=1..arc_resolution-1（不含兩端）
|
||||
for k in range(1, arc_resolution):
|
||||
t = k / arc_resolution
|
||||
theta = theta_in + sign * beta * t
|
||||
px = center[0] + R_actual * math.cos(theta)
|
||||
py = center[1] + R_actual * math.sin(theta)
|
||||
# 切線: sign × (-sin θ, cos θ)
|
||||
tx = sign * (-math.sin(theta))
|
||||
ty = sign * math.cos(theta)
|
||||
path.append({
|
||||
'seg': 'arc',
|
||||
'x': px, 'y': py,
|
||||
'dir': (tx, ty),
|
||||
'arc_center': center,
|
||||
'arc_R': R_actual,
|
||||
'arc_sign': sign,
|
||||
'theta': theta,
|
||||
})
|
||||
|
||||
path.append((bx, by, tdx, tdy))
|
||||
# T_out (straight, 方向 u_out)，barrier 放這
|
||||
path.append({
|
||||
'seg': 'straight',
|
||||
'x': T_out[0], 'y': T_out[1],
|
||||
'dir': u_out,
|
||||
})
|
||||
barrier_indices.append(len(path) - 1)
|
||||
|
||||
# 加入 post_turn 點 (方向 = outgoing)
|
||||
path.append((p2_x, p2_y, d_out_x, d_out_y))
|
||||
# 終點
|
||||
path.append({
|
||||
'seg': 'straight',
|
||||
'x': waypoints[-1][0], 'y': waypoints[-1][1],
|
||||
'dir': segments[-1][0],
|
||||
})
|
||||
|
||||
# 最後一個航點
|
||||
path.append((waypoints[-1][0], waypoints[-1][1],
|
||||
segments[-1][0], segments[-1][1]))
|
||||
# 後處理：為每個 sample 加上累積 polyline 弧長 s
|
||||
path[0]['s'] = 0.0
|
||||
for i in range(1, len(path)):
|
||||
prev = path[i - 1]
|
||||
cur = path[i]
|
||||
cur['s'] = prev['s'] + math.hypot(
|
||||
cur['x'] - prev['x'], cur['y'] - prev['y']
|
||||
)
|
||||
|
||||
return path
|
||||
return path, barrier_indices
|
||||
|
||||
# ------------------------------------------------------------------ 柵狀掃描
|
||||
|
||||
@@ -474,7 +631,15 @@ class FormationPlanner:
|
||||
|
||||
all_waypoints[original_idx] = waypoints_list
|
||||
|
||||
return all_waypoints
|
||||
# 每一條掃描線的「起點」都做一次同步：
|
||||
# wp 0 = gather, wp 1 = 第一條線起點, wp 2 = 第一條線終點,
|
||||
# wp 3 = 第二條線起點, wp 4 = 第二條線終點, ...
|
||||
# 每條線的「起點 index」= 1, 3, 5, ... = 2*i + 1（i 從 0 開始）
|
||||
# 所有 drone 的 waypoint 數量相同（num_lines 對所有 strip 都一致），
|
||||
# 所以用同一份 rendezvous_indices
|
||||
rendezvous_indices = [2 * i + 1 for i in range(num_lines)]
|
||||
|
||||
return all_waypoints, rendezvous_indices
|
||||
|
||||
|
||||
# ================================================================================
|
||||
@@ -492,12 +657,12 @@ if __name__ == "__main__":
|
||||
target = (24.12400, 120.56795, 10.0)
|
||||
|
||||
# M-Formation
|
||||
wps, origin = planner.plan_formation_mission(drones, target, MissionType.M_FORMATION)
|
||||
wps, origin, rv = planner.plan_formation_mission(drones, target, MissionType.M_FORMATION)
|
||||
print("M-Formation:")
|
||||
for i, wp_list in enumerate(wps):
|
||||
print(f" Drone {i}: {len(wp_list)} waypoints")
|
||||
|
||||
# Leader-Follower (Bezier 轉彎)
|
||||
# Leader-Follower (虛擬領隊 / 弧長參數化)
|
||||
route = [
|
||||
(24.12345, 120.56780),
|
||||
(24.12370, 120.56800),
|
||||
@@ -505,18 +670,18 @@ if __name__ == "__main__":
|
||||
(24.12400, 120.56800),
|
||||
(24.12420, 120.56790),
|
||||
]
|
||||
wps_lf, origin_lf = planner.plan_formation_mission(
|
||||
wps_lf, origin_lf, rv_lf = planner.plan_formation_mission(
|
||||
drones, target, MissionType.LEADER_FOLLOWER,
|
||||
params={
|
||||
'route_waypoints': route,
|
||||
'lateral_offset': 3.0,
|
||||
'longitudinal_spacing': 5.0,
|
||||
'turn_margin': 0.35,
|
||||
'curve_resolution': 8,
|
||||
'min_inner_radius': 3.0,
|
||||
'arc_resolution': 8,
|
||||
'altitude': 10.0
|
||||
}
|
||||
)
|
||||
print(f"\nLeader-Follower (Bezier turns):")
|
||||
print(f"\nLeader-Follower (arc-length virtual leader):")
|
||||
for i, wp_list in enumerate(wps_lf):
|
||||
print(f" Drone {i}: {len(wp_list)} waypoints")
|
||||
for j, wp in enumerate(wp_list):
|
||||
@@ -529,7 +694,7 @@ if __name__ == "__main__":
|
||||
(24.1240, 120.5683),
|
||||
(24.1240, 120.5679)
|
||||
]
|
||||
wps2, origin2 = planner.plan_formation_mission(
|
||||
wps2, origin2, rv2 = planner.plan_formation_mission(
|
||||
drones, target, MissionType.GRID_SWEEP,
|
||||
params={'rect_corners': rect, 'line_spacing': 5.0, 'altitude': 10.0}
|
||||
)
|
||||
|
||||
@@ -210,7 +210,7 @@ def main():
|
||||
target_gps = (target_lat, target_lon, BASE_ALTITUDE)
|
||||
print(f" 目標點: ({target_lat:.6f}, {target_lon:.6f})")
|
||||
|
||||
waypoints_per_drone, origin = planner.plan_formation_mission(
|
||||
waypoints_per_drone, origin, _ = planner.plan_formation_mission(
|
||||
drone_gps_positions, target_gps, MissionType.M_FORMATION
|
||||
)
|
||||
|
||||
@@ -232,7 +232,7 @@ def main():
|
||||
target_gps = (rect_center_lat, rect_center_lon, BASE_ALTITUDE)
|
||||
print(f" 矩形中心: ({rect_center_lat:.6f}, {rect_center_lon:.6f})")
|
||||
|
||||
waypoints_per_drone, origin = planner.plan_formation_mission(
|
||||
waypoints_per_drone, origin, _ = planner.plan_formation_mission(
|
||||
drone_gps_positions, target_gps, MissionType.GRID_SWEEP,
|
||||
params={
|
||||
'rect_corners': rect_corners,
|
||||
|
||||
@@ -365,7 +365,7 @@ def visualize_standalone():
|
||||
fig.suptitle('Mission Planner Verification (standalone)', fontsize=14, fontweight='bold')
|
||||
|
||||
# M-Formation
|
||||
wps_m, origin_m = planner.plan_formation_mission(drones, target, MissionType.M_FORMATION)
|
||||
wps_m, origin_m, _ = planner.plan_formation_mission(drones, target, MissionType.M_FORMATION)
|
||||
conv_m = CoordinateConverter(origin_m[0], origin_m[1], 0)
|
||||
data_m = {
|
||||
'drone_ids': [f's0_{i + 1}' for i in range(len(drones))],
|
||||
@@ -380,7 +380,7 @@ def visualize_standalone():
|
||||
# Grid Sweep 5m
|
||||
target_gs = (sum(c[0] for c in rect_corners) / 4,
|
||||
sum(c[1] for c in rect_corners) / 4, 10.0)
|
||||
wps_g, origin_g = planner.plan_formation_mission(
|
||||
wps_g, origin_g, _ = planner.plan_formation_mission(
|
||||
drones, target_gs, MissionType.GRID_SWEEP,
|
||||
params={'rect_corners': rect_corners, 'line_spacing': 5.0, 'altitude': 10.0}
|
||||
)
|
||||
@@ -403,7 +403,7 @@ def visualize_standalone():
|
||||
(24.12410, 120.56800),
|
||||
(24.12420, 120.56790),
|
||||
]
|
||||
wps_lf, origin_lf = planner.plan_formation_mission(
|
||||
wps_lf, origin_lf, _ = planner.plan_formation_mission(
|
||||
drones, target, MissionType.LEADER_FOLLOWER,
|
||||
params={'route_waypoints': route, 'lateral_offset': 3.0,
|
||||
'longitudinal_spacing': 5.0, 'altitude': 10.0}
|
||||
|
||||
@@ -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:
|
||||
@@ -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,
|
||||
)
|
||||
|
||||
Reference in New Issue
Block a user