|
|
|
|
|
"""ROS2 client for fc_network MavPositionTargetGlobalInt service (SET_POSITION_TARGET_GLOBAL_INT / Offboard)."""
|
|
|
|
|
|
|
|
|
|
|
|
from __future__ import annotations
|
|
|
|
|
|
|
|
|
|
|
|
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) -> None:
|
|
|
|
|
|
if not rclpy.ok():
|
|
|
|
|
|
rclpy.init(args=None)
|
|
|
|
|
|
super().__init__("fc_position_target_global_int_client")
|
|
|
|
|
|
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 set_position_target_global_int(
|
|
|
|
|
|
# self,
|
|
|
|
|
|
# *,
|
|
|
|
|
|
# target_sysid: int,
|
|
|
|
|
|
# target_compid: int = 0,
|
|
|
|
|
|
# time_boot_ms: int = 0,
|
|
|
|
|
|
# coordinate_frame: int = 6,
|
|
|
|
|
|
# type_mask: int = 0,
|
|
|
|
|
|
# lat_int: int = 0,
|
|
|
|
|
|
# lon_int: int = 0,
|
|
|
|
|
|
# alt: float = 0.0,
|
|
|
|
|
|
# vx: float = 0.0,
|
|
|
|
|
|
# vy: float = 0.0,
|
|
|
|
|
|
# vz: float = 0.0,
|
|
|
|
|
|
# afx: float = 0.0,
|
|
|
|
|
|
# afy: float = 0.0,
|
|
|
|
|
|
# afz: float = 0.0,
|
|
|
|
|
|
# yaw: float = 0.0,
|
|
|
|
|
|
# yaw_rate: float = 0.0,
|
|
|
|
|
|
# timeout_sec: float = DEFAULT_TIMEOUT_SEC,
|
|
|
|
|
|
# ) -> PositionTargetGlobalIntResult:
|
|
|
|
|
|
# """
|
|
|
|
|
|
# 送出 SET_POSITION_TARGET_GLOBAL_INT；coordinate_frame 預設 6 (MAV_FRAME_GLOBAL_INT)
|
|
|
|
|
|
# 可依飛控／任務覆寫。
|
|
|
|
|
|
# """
|
|
|
|
|
|
# return self._send_position_target_global_int(
|
|
|
|
|
|
# target_sysid=target_sysid,
|
|
|
|
|
|
# target_compid=target_compid,
|
|
|
|
|
|
# time_boot_ms=time_boot_ms,
|
|
|
|
|
|
# coordinate_frame=coordinate_frame,
|
|
|
|
|
|
# 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,
|
|
|
|
|
|
# timeout_sec=timeout_sec,
|
|
|
|
|
|
# )
|
|
|
|
|
|
|
|
|
|
|
|
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,
|
|
|
|
|
|
)
|
|
|
|
|
|
|
|
|
|
|
|
# def set_position_target_global_int_deg(
|
|
|
|
|
|
# self,
|
|
|
|
|
|
# *,
|
|
|
|
|
|
# target_sysid: int,
|
|
|
|
|
|
# latitude_deg: float,
|
|
|
|
|
|
# longitude_deg: float,
|
|
|
|
|
|
# target_compid: int = 0,
|
|
|
|
|
|
# time_boot_ms: int = 0,
|
|
|
|
|
|
# coordinate_frame: int = MAV_FRAME_GLOBAL_RELATIVE_ALT_INT,
|
|
|
|
|
|
# type_mask: int = 0,
|
|
|
|
|
|
# alt: float = 0.0,
|
|
|
|
|
|
# vx: float = 0.0,
|
|
|
|
|
|
# vy: float = 0.0,
|
|
|
|
|
|
# vz: float = 0.0,
|
|
|
|
|
|
# afx: float = 0.0,
|
|
|
|
|
|
# afy: float = 0.0,
|
|
|
|
|
|
# afz: float = 0.0,
|
|
|
|
|
|
# yaw: float = 0.0,
|
|
|
|
|
|
# yaw_rate: float = 0.0,
|
|
|
|
|
|
# timeout_sec: float = DEFAULT_TIMEOUT_SEC,
|
|
|
|
|
|
# ) -> PositionTargetGlobalIntResult:
|
|
|
|
|
|
# """以度為單位的緯經，轉成 MAVLink 的 lat_int / lon_int (1e7 * deg)。"""
|
|
|
|
|
|
# lat_int = int(round(latitude_deg * 1e7))
|
|
|
|
|
|
# lon_int = int(round(longitude_deg * 1e7))
|
|
|
|
|
|
# return self.set_position_target_global_int(
|
|
|
|
|
|
# target_sysid=target_sysid,
|
|
|
|
|
|
# target_compid=target_compid,
|
|
|
|
|
|
# time_boot_ms=time_boot_ms,
|
|
|
|
|
|
# coordinate_frame=coordinate_frame,
|
|
|
|
|
|
# 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,
|
|
|
|
|
|
# timeout_sec=timeout_sec,
|
|
|
|
|
|
# )
|
