wenchunandClaude Opus 4.6
0edc477df8
- 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>
419 lines
14 KiB
Python
419 lines
14 KiB
Python
"""ROS2 client for fc_network MavPositionTargetGlobalInt service (SET_POSITION_TARGET_GLOBAL_INT / Offboard)."""
|
||
|
||
from __future__ import annotations
|
||
|
||
import asyncio
|
||
import math
|
||
from dataclasses import dataclass
|
||
|
||
import rclpy
|
||
from rclpy.node import Node
|
||
|
||
from fc_interfaces.srv import MavPositionTargetGlobalInt
|
||
|
||
DEFAULT_SERVICE_NAME = "/fc_network/vehicle/pos_global_int"
|
||
DEFAULT_TIMEOUT_SEC = 2.0
|
||
|
||
# MAV_FRAME
|
||
MAV_FRAME_GLOBAL_RELATIVE_ALT_INT = 6 # 使用 WGS84 GNSS 座標系 高度為相對於 Home 點高度
|
||
|
||
# POSITION_TARGET_TYPEMASK
|
||
# 1 1 Ignore position x
|
||
# 2 2 Ignore position y
|
||
# 3 4 Ignore position z
|
||
# 4 8 Ignore velocity x
|
||
# 5 16 Ignore velocity y
|
||
# 6 32 Ignore velocity z
|
||
# 7 64 Ignore acceleration x
|
||
# 8 128 Ignore acceleration y
|
||
# 9 256 Ignore acceleration z
|
||
# 10 512 Use force instead of acceleration
|
||
# 11 1024 Ignore yaw
|
||
# 12 2048 Ignore yaw rate
|
||
# 13+ Not Used
|
||
# 6543210987654321
|
||
POSITION_ONLY = 0b0000110111111000 # 忽略速度、加速度、偏航
|
||
|
||
|
||
# 回傳 echo 與送出值比對時，浮點欄位允許的絕對誤差（MAVLink float 轉傳常有小差）
|
||
_FLOAT_ECHO_ABS_TOL = 1e-5
|
||
def _float_echo_equal(a: float, b: float) -> bool:
|
||
return math.isclose(float(a), float(b), rel_tol=0.0, abs_tol=_FLOAT_ECHO_ABS_TOL)
|
||
|
||
|
||
def _echo_position_target_matches(
|
||
*,
|
||
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,
|
||
time_boot_ms: int,
|
||
resp,
|
||
) -> tuple[bool, str]:
|
||
"""
|
||
比對本次送出的軌跡欄位與 server 填回的 r_* 是否一致。
|
||
若不符，回傳 (False, 簡短說明)；相符則 (True, "")。
|
||
"""
|
||
|
||
_tm_sent = int(type_mask) & 0xFFF
|
||
_tm_recv = int(resp.r_type_mask) & 0xFFF
|
||
if _tm_recv != _tm_sent:
|
||
return False, f"type_mask echo: sent {_tm_sent:012b} != ack {_tm_recv:012b}"
|
||
|
||
coordinate_tolerance = 5 * 100
|
||
if abs(resp.r_lat_int - lat_int) >= coordinate_tolerance:
|
||
return False, f"lat_int echo: sent {lat_int} != ack {resp.r_lat_int}"
|
||
if abs(resp.r_lon_int - lon_int) >= coordinate_tolerance:
|
||
return False, f"lon_int echo: sent {lon_int} != ack {resp.r_lon_int}"
|
||
# TODO 我給飛控相對高度 它回給我絕對高度 看來這個比較方法 還有很多改良點 但是之後吧
|
||
# if not _float_echo_equal(resp.r_alt, alt):
|
||
# return False, f"alt echo: sent {alt} != ack {resp.r_alt}"
|
||
if not _float_echo_equal(resp.r_vx, vx):
|
||
return False, f"vx echo: sent {vx} != ack {resp.r_vx}"
|
||
if not _float_echo_equal(resp.r_vy, vy):
|
||
return False, f"vy echo: sent {vy} != ack {resp.r_vy}"
|
||
if not _float_echo_equal(resp.r_vz, vz):
|
||
return False, f"vz echo: sent {vz} != ack {resp.r_vz}"
|
||
if not _float_echo_equal(resp.r_afx, afx):
|
||
return False, f"afx echo: sent {afx} != ack {resp.r_afx}"
|
||
if not _float_echo_equal(resp.r_afy, afy):
|
||
return False, f"afy echo: sent {afy} != ack {resp.r_afy}"
|
||
if not _float_echo_equal(resp.r_afz, afz):
|
||
return False, f"afz echo: sent {afz} != ack {resp.r_afz}"
|
||
if not _float_echo_equal(resp.r_yaw, yaw):
|
||
return False, f"yaw echo: sent {yaw} != ack {resp.r_yaw}"
|
||
if not _float_echo_equal(resp.r_yaw_rate, yaw_rate):
|
||
return False, f"yaw_rate echo: sent {yaw_rate} != ack {resp.r_yaw_rate}"
|
||
# if int(resp.r_time_boot_ms) != int(time_boot_ms):
|
||
# return False, f"time_boot_ms echo: sent {time_boot_ms} != r {resp.r_time_boot_ms}"
|
||
return True, ""
|
||
|
||
|
||
@dataclass
|
||
class PositionTargetGlobalIntResult:
|
||
"""對應 MavPositionTargetGlobalInt.srv 的 Response；success 依「送出與 r_* echo 是否一致」。"""
|
||
success: bool
|
||
message: str
|
||
ack_result: int
|
||
r_time_boot_ms: int = 0
|
||
r_type_mask: int = 0
|
||
r_lat_int: int = 0
|
||
r_lon_int: int = 0
|
||
r_alt: float = 0.0
|
||
r_vx: float = 0.0
|
||
r_vy: float = 0.0
|
||
r_vz: float = 0.0
|
||
r_afx: float = 0.0
|
||
r_afy: float = 0.0
|
||
r_afz: float = 0.0
|
||
r_yaw: float = 0.0
|
||
r_yaw_rate: float = 0.0
|
||
|
||
class PositionTargetGlobalIntClient(Node):
|
||
"""
|
||
ROS2 client：對 pos_global_int service 發送 SET_POSITION_TARGET_GLOBAL_INT (msg 86)，
|
||
並等待 POSITION_TARGET_GLOBAL_INT (msg 87) 回應（由 server 端處理）。
|
||
"""
|
||
|
||
def __init__(self, service_name: str = DEFAULT_SERVICE_NAME, node_name: str = None) -> None:
|
||
if not rclpy.ok():
|
||
rclpy.init(args=None)
|
||
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:
|
||
return self._client.wait_for_service(timeout_sec=timeout_sec)
|
||
|
||
def _send_position_target_global_int(
|
||
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:
|
||
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)
|
||
rclpy.spin_until_future_complete(self, future, timeout_sec=float(timeout_sec) + 1.0)
|
||
if not future.done() or future.result() is None:
|
||
return PositionTargetGlobalIntResult(
|
||
success=False,
|
||
message="Service timeout.",
|
||
ack_result=-1,
|
||
)
|
||
|
||
resp = future.result()
|
||
|
||
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,
|
||
)
|
||
# out_message = resp.message
|
||
# if not echo_ok:
|
||
# out_message = echo_detail if not out_message else f"{echo_detail} | server: {out_message}"
|
||
|
||
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),
|
||
)
|
||
|
||
def goto_position_only(
|
||
self,
|
||
*,
|
||
target_sysid: int,
|
||
target_compid: int = 0,
|
||
latitude_deg: float,
|
||
longitude_deg: float,
|
||
alt: float = 0.0,
|
||
timeout_sec: float,
|
||
):
|
||
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,
|
||
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,
|
||
)
|
||
|
||
# ============================================================================
|
||
# 非阻塞 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,
|
||
)
|