chiyu1468/AirTrapMine
2
0
Fork 1
Code Issues Pull Requests Packages Projects Releases Wiki Activity

(tested)

Browse Source 
1. 新增 ros2 接收 SET_POSITION_TARGET_LOCAL_NED 功能, 因應新增 MavPositionTargetGlobalInt.srv 的介面定義檔, 並相應新增使用者的外層包裝 navigation.py
2. 改善 ros2 service 的 response ack 數值對應, 新增 ackEnum.py
3. 改善 longCommand.py 內的呼叫層次, 把 longCommand 相關整合到同樣的 class 下, 僅以 def function 區分內部套用那種 MAV_CMD
4. 新增 example_position_goto.py, example_takeoff_land.py 作為使用範例 ( land.py arm_disarm.py ... 這些下個版本會被刪除)
5. 剩下就是一些註解的修正與更新
This commit is contained in:
chiyu1468
2026-04-07 12:28:11 +08:00
parent 44d53f51fb
commit eb2f0af58e
11 changed files with 848 additions and 61 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/fc_network_apps/navigation.py
+353
View File
@@ -0,0 +1,353 @@
"""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,
# )