Merge branch 'chiyu'
1. 初步完善 ros2 service 功能 包含解鎖起飛降落與 goto 2. 為了整合到 GUI 去使用
This commit is contained in:
@@ -25,7 +25,7 @@ ROS2
|
||||
功能簡介
|
||||
1. mavlink 多對多支援平台
|
||||
2. 不允許進到 ros 系統有相同 sysid
|
||||
3. 假設所有 component 共用同一 socket
|
||||
3. 假設一台載具上所有 component 共用同一 socket
|
||||
|
||||
===
|
||||
開發用輔助專案
|
||||
@@ -65,16 +65,18 @@ Package 簡述
|
||||
02 -> 其宇(chiyu)
|
||||
03 -> 文鈞
|
||||
04 -> 倫渝
|
||||
2. fc_network_adapter
|
||||
2. fc_network_adapter (核心)
|
||||
建立、維護與飛控韌體的連接
|
||||
構築 mavlink 封包
|
||||
處理無線模組的通訊格式 (XBee)
|
||||
--同時處理與 Gazebo 的 ardupilot_plugin 溝通的 FDM/JSON 訊息 (移除)--
|
||||
3. fc_interfaces
|
||||
自定義的介面檔
|
||||
3. fc_interfaces (重要)
|
||||
自定義的 ROS2 介面檔
|
||||
4. fc_network_apps
|
||||
與 fc_network_adapter 銜接做高階功能包裝的應用小程式 利於開發GUI或其他應用
|
||||
|
||||
使用者的外層包裝
|
||||
5. someotherpkg
|
||||
如何使用 fc_network_apps 的範例檔案
|
||||
N. logs 是執行時期的記錄檔
|
||||
|
||||
===
|
||||
|
||||
@@ -12,6 +12,7 @@ find_package(rosidl_default_generators REQUIRED)
|
||||
rosidl_generate_interfaces(${PROJECT_NAME}
|
||||
"srv/MavPing.srv"
|
||||
"srv/MavCommandLong.srv"
|
||||
"srv/MavPositionTargetGlobalInt.srv"
|
||||
)
|
||||
|
||||
ament_package()
|
||||
|
||||
@@ -0,0 +1,36 @@
|
||||
# Request
|
||||
uint8 target_sysid
|
||||
uint8 target_compid
|
||||
|
||||
uint32 time_boot_ms # ms since system boot, 一般用 0 讓飛控自己填
|
||||
uint8 coordinate_frame # MAV_FRAME_*
|
||||
uint16 type_mask # POSITION_TARGET_TYPEMASK bitmask
|
||||
int32 lat_int # 1e7 * latitude (deg)
|
||||
int32 lon_int # 1e7 * longitude (deg)
|
||||
float32 alt # altitude (m)
|
||||
float32 vx # X velocity (m/s)
|
||||
float32 vy # Y velocity (m/s)
|
||||
float32 vz # Z velocity (m/s)
|
||||
float32 afx # X acceleration or force (m/s^2)
|
||||
float32 afy # Y acceleration or force (m/s^2)
|
||||
float32 afz # Z acceleration or force (m/s^2)
|
||||
float32 yaw # yaw (rad)
|
||||
float32 yaw_rate # yaw rate (rad/s)
|
||||
float32 timeout_sec # 等待 POSITION_TARGET_GLOBAL_INT 回應的時間上限
|
||||
---
|
||||
# Response
|
||||
string message
|
||||
uint8 ack_result
|
||||
uint32 r_time_boot_ms
|
||||
uint16 r_type_mask
|
||||
int32 r_lat_int
|
||||
int32 r_lon_int
|
||||
float32 r_alt
|
||||
float32 r_vx
|
||||
float32 r_vy
|
||||
float32 r_vz
|
||||
float32 r_afx
|
||||
float32 r_afy
|
||||
float32 r_afz
|
||||
float32 r_yaw
|
||||
float32 r_yaw_rate
|
||||
@@ -0,0 +1,31 @@
|
||||
|
||||
'''
|
||||
單獨出來定義回傳碼的意義
|
||||
若跟飛控韌體有正常的通訊 (無論是否被拒絕) 則以 mavlink 定義的結果回傳
|
||||
若跟飛控韌體無法通訊 才使用這邊的錯誤代碼
|
||||
所以要避開重疊的號碼
|
||||
|
||||
這邊備註 Mavlink 的 MAV_RESULT 參數意義
|
||||
0 : 封包正常接受並執行
|
||||
1 : 封包正常接受 但是正在忙不執行 需要等待後重試
|
||||
4 : 封包正常接受 但是載具的狀態異常 不執行
|
||||
|
||||
2 : 封包不正常 參數可能給錯了
|
||||
3 : 封包不正常 根本認不出是啥
|
||||
|
||||
'''
|
||||
|
||||
|
||||
from enum import IntEnum
|
||||
|
||||
class serviceAckResult(IntEnum):
|
||||
MAVLINK_TIMEOUT = 51 # mavlink communication timeout
|
||||
|
||||
MAVLINK_SEND_BUT_NO_EXP_STEAM = 30 # some messages will not have obviously ack, so monitering feature message. however, we do NOT get the feature message. ex. 85/86 pair
|
||||
|
||||
MAVLINK_BUSY = 53 # there already has a command being execution for this sysid, try later
|
||||
MAVLINK_DEV_NOTFOUND = 54 # this sysid not fount
|
||||
|
||||
UNKNOWN = 90 # i did not capture this error, try read the logs
|
||||
|
||||
|
||||
@@ -29,7 +29,7 @@ from .utils import acquireSerial, acquirePort
|
||||
from .utils.acquirePort import find_available_port
|
||||
|
||||
logger = setup_logger(os.path.basename(__file__))
|
||||
VERSION_NO = "v0.60"
|
||||
VERSION_NO = "v0.61"
|
||||
|
||||
class PanelState:
|
||||
def __init__(self):
|
||||
|
||||
@@ -1,10 +1,15 @@
|
||||
"""
|
||||
MAVLink ROS2 Nodes
|
||||
包含兩個獨立的 ROS2 Node：
|
||||
主要包含兩個獨立的 ROS2 Node :
|
||||
1. VehicleStatusPublisher - 發布載具狀態到 ROS2 topics
|
||||
從 vehicle_registry 讀取狀態數據，頻率控制，模組化設計
|
||||
2. MavlinkCommandService - 提供 MAVLink 指令 service 介面
|
||||
以最基礎的 mavlink microservice 會實現目標
|
||||
並不會包含額外的功能
|
||||
|
||||
與一個節點管理器
|
||||
- fc_ros_manager
|
||||
|
||||
從 vehicle_registry 讀取狀態數據，頻率控制，模組化設計
|
||||
"""
|
||||
|
||||
import os
|
||||
@@ -26,6 +31,7 @@ import mavros_msgs.msg
|
||||
|
||||
# ROS2 Service imports
|
||||
import fc_interfaces.srv as fcsrv
|
||||
from .ackEnum import serviceAckResult
|
||||
|
||||
# 自定義 imports
|
||||
from . import mavlinkVehicleView as mvv
|
||||
@@ -521,6 +527,15 @@ class MavlinkCommandService(Node):
|
||||
self.handle_command_long
|
||||
)
|
||||
|
||||
# ═══════════════════════════════════════════════════════════════════
|
||||
# Service : 發送 SET_POSITION_TARGET_GLOBAL_INT (86)
|
||||
# ═══════════════════════════════════════════════════════════════════
|
||||
self.srv_pos_global_int = self.create_service(
|
||||
fcsrv.MavPositionTargetGlobalInt,
|
||||
self.serviceString_prefix + '/pos_global_int',
|
||||
self.handle_set_position_target_global_int
|
||||
)
|
||||
|
||||
logger.info("MavlinkCommandService initialized")
|
||||
|
||||
def _index(self, target_sysid, target_compid):
|
||||
@@ -568,6 +583,7 @@ class MavlinkCommandService(Node):
|
||||
def handle_mav_timesync_ping(self, request, response):
|
||||
'''
|
||||
用 timesync 封包驗證來回的時間
|
||||
隸屬於 MavLink PING Protocol
|
||||
'''
|
||||
# 設定失效回應
|
||||
response.success = False
|
||||
@@ -612,7 +628,7 @@ class MavlinkCommandService(Node):
|
||||
fail_skip = False
|
||||
# 5) 等待回應封包
|
||||
if not evt.wait(timeout_sec):
|
||||
response.message = "waiting Timeout - TIMESYNC"
|
||||
response.message = "mavlink timeout - TIMESYNC"
|
||||
fail_skip = True
|
||||
|
||||
# 6) 處理回應封包
|
||||
@@ -633,35 +649,31 @@ class MavlinkCommandService(Node):
|
||||
|
||||
return response
|
||||
|
||||
def handle_add_two_ints(self, request, response):
|
||||
"""測試用 Service Handler 未來刪除"""
|
||||
logger.info(f"Received add_two_ints request: {request.a} + {request.b}")
|
||||
response.sum = request.a + request.b
|
||||
logger.info(
|
||||
f"[add_two_ints] thread_ident={threading.get_ident()} time={time.time()}"
|
||||
)
|
||||
time.sleep(1)
|
||||
|
||||
return response
|
||||
|
||||
# ═══════════════════════════════════════════════════════════════════════
|
||||
# Service Handler: 發送 COMMAND_LONG
|
||||
# Service Handler: 發送 COMMAND_LONG (76)
|
||||
# ═══════════════════════════════════════════════════════════════════════
|
||||
def handle_command_long(self, request, response):
|
||||
'''
|
||||
用 command long 丟出 MAV_CMD
|
||||
隸屬於 MavLink Command Protocol
|
||||
'''
|
||||
# 設定失效回應
|
||||
response.success = False
|
||||
response.message = "Unknown error"
|
||||
response.ack_result = serviceAckResult.UNKNOWN
|
||||
timeout_sec = request.timeout_sec
|
||||
|
||||
# 1) 確認是否已經有相同 sysid 的其他需求正在 pending
|
||||
if request.target_sysid in self._pending_by_sysid:
|
||||
response.message = f"sysid {request.target_sysid} already has pending request"
|
||||
response.ack_result = serviceAckResult.MAVLINK_BUSY
|
||||
return response
|
||||
|
||||
# 2) 找到 socket 標地
|
||||
socketObject = self._index(request.target_sysid, request.target_compid)
|
||||
if socketObject is None:
|
||||
response.message = "This system id not found."
|
||||
response.ack_result = serviceAckResult.MAVLINK_DEV_NOTFOUND
|
||||
return response
|
||||
|
||||
# 3) 接收封包系統 的設定
|
||||
@@ -691,7 +703,8 @@ class MavlinkCommandService(Node):
|
||||
fail_skip = False
|
||||
# 5) 等待回應封包
|
||||
if not evt.wait(timeout_sec):
|
||||
response.message = "waiting Timeout - CommLONG"
|
||||
response.message = "mavlink timeout - CommLONG"
|
||||
response.ack_result = serviceAckResult.MAVLINK_TIMEOUT
|
||||
fail_skip = True
|
||||
|
||||
# 6) 處理回應封包
|
||||
@@ -711,6 +724,95 @@ class MavlinkCommandService(Node):
|
||||
|
||||
return response
|
||||
|
||||
# ═══════════════════════════════════════════════════════════════════════
|
||||
# Service Handler: 發送 SET_POSITION_TARGET_GLOBAL_INT (86)
|
||||
# ═══════════════════════════════════════════════════════════════════════
|
||||
def handle_set_position_target_global_int(self, request, response):
|
||||
'''
|
||||
隸屬於 MavLink Offboard Control Protocol
|
||||
'''
|
||||
# 設定失效回應
|
||||
response.message = "Unknown error"
|
||||
response.ack_result = serviceAckResult.UNKNOWN
|
||||
timeout_sec = request.timeout_sec
|
||||
|
||||
# 1) 確認是否已經有相同 sysid 的其他需求正在 pending
|
||||
if request.target_sysid in self._pending_by_sysid:
|
||||
response.message = f"sysid {request.target_sysid} already has pending request"
|
||||
response.ack_result = serviceAckResult.MAVLINK_BUSY
|
||||
return response
|
||||
|
||||
# 2) 找到 socket 標地
|
||||
socketObject = self._index(request.target_sysid, request.target_compid)
|
||||
if socketObject is None:
|
||||
response.message = "This system id not found."
|
||||
response.ack_result = serviceAckResult.MAVLINK_DEV_NOTFOUND
|
||||
return response
|
||||
|
||||
# 3) 接收封包系統 的設定
|
||||
# 在 socket 那邊先把要的封包種類導流進來
|
||||
expect_recieve_msg_id = 87 # POSITION_TARGET_GLOBAL_INT
|
||||
socketObject.set_return_message_types(list(socketObject.return_msg_types) + [expect_recieve_msg_id])
|
||||
evt = threading.Event()
|
||||
# 設定封包檢驗
|
||||
def match_fn(compare_msg):
|
||||
if compare_msg.get_msgId() != expect_recieve_msg_id :
|
||||
return False
|
||||
return True
|
||||
|
||||
# 4) 送出封包
|
||||
socketObject.MAVLink.set_position_target_global_int_send(
|
||||
request.time_boot_ms,
|
||||
request.target_sysid,
|
||||
request.target_compid,
|
||||
request.coordinate_frame,
|
||||
request.type_mask,
|
||||
request.lat_int, request.lon_int, request.alt,
|
||||
request.vx, request.vy, request.vz,
|
||||
request.afx, request.afy, request.afz,
|
||||
request.yaw, request.yaw_rate
|
||||
)
|
||||
|
||||
time.sleep(0.01) # 因應 mavlink 的廣播式回應 後置回應監聽 並且故意延遲10ms 讓系統代謝掉舊的87封包
|
||||
_pending = PendingEntry(event = evt, match_fn = match_fn)
|
||||
self._pending_by_sysid[request.target_sysid] = _pending
|
||||
|
||||
fail_skip = False
|
||||
# 5) 等待回應封包
|
||||
if not evt.wait(timeout_sec):
|
||||
response.message = "mavlink timeout - SET POSITION 86"
|
||||
response.ack_result = MAVLINK_SEND_BUT_NO_EXP_STEAM
|
||||
fail_skip = True
|
||||
|
||||
# 6) 處理回應封包
|
||||
if not fail_skip:
|
||||
ack_msg = _pending.result_mav_msg
|
||||
|
||||
response.message = ""
|
||||
response.ack_result = 0
|
||||
response.r_time_boot_ms = ack_msg.time_boot_ms
|
||||
response.r_type_mask = ack_msg.type_mask
|
||||
response.r_lat_int = ack_msg.lat_int
|
||||
response.r_lon_int = ack_msg.lon_int
|
||||
response.r_alt = ack_msg.alt
|
||||
response.r_vx = ack_msg.vx
|
||||
response.r_vy = ack_msg.vy
|
||||
response.r_vz = ack_msg.vz
|
||||
response.r_afx = ack_msg.afx
|
||||
response.r_afy = ack_msg.afy
|
||||
response.r_afz = ack_msg.afz
|
||||
response.r_yaw = ack_msg.yaw
|
||||
response.r_yaw_rate = ack_msg.yaw_rate
|
||||
|
||||
# 7) 接收封包系統 的重置
|
||||
msg_types = list(socketObject.return_msg_types)
|
||||
if expect_recieve_msg_id in msg_types:
|
||||
msg_types.remove(expect_recieve_msg_id)
|
||||
socketObject.set_return_message_types(msg_types)
|
||||
del evt
|
||||
self._pending_by_sysid.pop(request.target_sysid, None)
|
||||
|
||||
return response
|
||||
|
||||
# ═══════════════════════════════════════════════════════════════════════
|
||||
# Service Handler: 參數請求
|
||||
@@ -791,6 +893,20 @@ class MavlinkCommandService(Node):
|
||||
response.message = "Param request sent (placeholder implementation)"
|
||||
return response
|
||||
|
||||
|
||||
def handle_add_two_ints(self, request, response):
|
||||
"""測試用 Service Handler 未來刪除"""
|
||||
logger.info(f"Received add_two_ints request: {request.a} + {request.b}")
|
||||
response.sum = request.a + request.b
|
||||
logger.info(
|
||||
f"[add_two_ints] thread_ident={threading.get_ident()} time={time.time()}"
|
||||
)
|
||||
time.sleep(1)
|
||||
|
||||
return response
|
||||
|
||||
|
||||
|
||||
def stop(self):
|
||||
"""停止服務"""
|
||||
self.running = False
|
||||
@@ -973,7 +1089,11 @@ ros2_manager = fc_ros_manager()
|
||||
5. 預留 MavlinkCommandService 結構（稍後實現）
|
||||
|
||||
2026.03.27
|
||||
1. 完成 ros2 service 結構與 timesync command_long 協議
|
||||
1. 完成 ros2 service 結構與 timesync 與 command_long 協議
|
||||
|
||||
2026.04.07
|
||||
1. 完成 ros2 的 MavPositionTargetGlobalInt 區域
|
||||
2. 優化 response.ack_result 回傳值的資訊
|
||||
|
||||
'''
|
||||
|
||||
|
||||
@@ -1,3 +1,4 @@
|
||||
"""ROS2 client for fc_network MavCommandLong service (COMMAND_LONG 系列指令)."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
@@ -10,30 +11,82 @@ from rclpy.node import Node
|
||||
from fc_interfaces.srv import MavCommandLong
|
||||
|
||||
COMMAND_DO_SET_MODE = 176
|
||||
COMMAND_NAV_LAND = 21
|
||||
COMMAND_NAV_TAKEOFF = 22
|
||||
COMMAND_COMPONENT_ARM_DISARM = 400
|
||||
|
||||
DEFAULT_SERVICE_NAME = "/fc_network/vehicle/send_command_long"
|
||||
DEFAULT_TIMEOUT_SEC = 2.0
|
||||
|
||||
|
||||
@dataclass
|
||||
class ChangeModeResult:
|
||||
"""Return value for mode change requests."""
|
||||
|
||||
class CommandLongResult:
|
||||
success: bool
|
||||
message: str
|
||||
ack_result: int
|
||||
|
||||
|
||||
ChangeModeResult = CommandLongResult
|
||||
|
||||
|
||||
class CommandLongClient(Node):
|
||||
"""Small ROS2 client dedicated to change flight mode."""
|
||||
"""ROS2 client : 對同一個 send_command_long service 發送各種 MAV_CMD_*。"""
|
||||
|
||||
def __init__(self, service_name: str = DEFAULT_SERVICE_NAME) -> None:
|
||||
rclpy.init()
|
||||
super().__init__("fc_change_mode_client")
|
||||
if not rclpy.ok():
|
||||
rclpy.init(args=None)
|
||||
super().__init__("fc_command_long_client")
|
||||
self._client = self.create_client(MavCommandLong, 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_command_long(
|
||||
self,
|
||||
*,
|
||||
target_sysid: int,
|
||||
target_compid: int,
|
||||
command: int,
|
||||
confirmation: int,
|
||||
param1: float,
|
||||
param2: float,
|
||||
param3: float,
|
||||
param4: float,
|
||||
param5: float,
|
||||
param6: float,
|
||||
param7: float,
|
||||
timeout_sec: float,
|
||||
) -> CommandLongResult:
|
||||
req = MavCommandLong.Request()
|
||||
req.target_sysid = target_sysid
|
||||
req.target_compid = target_compid
|
||||
req.command = command
|
||||
req.confirmation = confirmation
|
||||
req.param1 = param1
|
||||
req.param2 = param2
|
||||
req.param3 = param3
|
||||
req.param4 = param4
|
||||
req.param5 = param5
|
||||
req.param6 = param6
|
||||
req.param7 = param7
|
||||
req.timeout_sec = float(timeout_sec)
|
||||
|
||||
future = self._client.call_async(req)
|
||||
rclpy.spin_until_future_complete(self, future, timeout_sec=timeout_sec + 1.0)
|
||||
if not future.done() or future.result() is None:
|
||||
return CommandLongResult(
|
||||
success=False,
|
||||
message="Service timeout.",
|
||||
ack_result=-1,
|
||||
)
|
||||
|
||||
response = future.result()
|
||||
return CommandLongResult(
|
||||
success=response.success,
|
||||
message=response.message,
|
||||
ack_result=response.ack_result,
|
||||
)
|
||||
|
||||
def change_mode(
|
||||
self,
|
||||
*,
|
||||
@@ -42,34 +95,97 @@ class CommandLongClient(Node):
|
||||
target_compid: int = 0,
|
||||
base_mode: float = 1.0,
|
||||
confirmation: int = 0,
|
||||
timeout_sec: float = DEFAULT_TIMEOUT_SEC ) -> ChangeModeResult:
|
||||
timeout_sec: float = DEFAULT_TIMEOUT_SEC,
|
||||
) -> CommandLongResult:
|
||||
return self._send_command_long(
|
||||
target_sysid=target_sysid,
|
||||
target_compid=target_compid,
|
||||
command=COMMAND_DO_SET_MODE,
|
||||
confirmation=confirmation,
|
||||
param1=float(base_mode),
|
||||
param2=float(custom_mode),
|
||||
param3=0.0,
|
||||
param4=0.0,
|
||||
param5=0.0,
|
||||
param6=0.0,
|
||||
param7=0.0,
|
||||
timeout_sec=timeout_sec,
|
||||
)
|
||||
|
||||
req = MavCommandLong.Request()
|
||||
req.target_sysid = target_sysid
|
||||
req.target_compid = target_compid
|
||||
req.command = COMMAND_DO_SET_MODE
|
||||
req.confirmation = confirmation
|
||||
req.param1 = float(base_mode)
|
||||
req.param2 = float(custom_mode)
|
||||
req.param3 = 0.0
|
||||
req.param4 = 0.0
|
||||
req.param5 = 0.0
|
||||
req.param6 = 0.0
|
||||
req.param7 = 0.0
|
||||
req.timeout_sec = float(timeout_sec)
|
||||
def takeoff(
|
||||
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:
|
||||
return self._send_command_long(
|
||||
target_sysid=target_sysid,
|
||||
target_compid=target_compid,
|
||||
command=COMMAND_NAV_TAKEOFF,
|
||||
confirmation=0,
|
||||
param1=float(min_pitch_deg),
|
||||
param2=0.0,
|
||||
param3=0.0,
|
||||
param4=float(yaw_deg),
|
||||
param5=float(latitude) if latitude is not None else 0.0,
|
||||
param6=float(longitude) if longitude is not None else 0.0,
|
||||
param7=float(altitude_m),
|
||||
timeout_sec=timeout_sec,
|
||||
)
|
||||
|
||||
future = self._client.call_async(req)
|
||||
rclpy.spin_until_future_complete(self, future, timeout_sec=timeout_sec + 1.0)
|
||||
if not future.done() or future.result() is None:
|
||||
return ChangeModeResult(
|
||||
success=False,
|
||||
message="Service call timeout or no response.",
|
||||
ack_result=-1,
|
||||
)
|
||||
def land(
|
||||
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:
|
||||
return self._send_command_long(
|
||||
target_sysid=target_sysid,
|
||||
target_compid=target_compid,
|
||||
command=COMMAND_NAV_LAND,
|
||||
confirmation=0,
|
||||
param1=0.0,
|
||||
param2=0.0,
|
||||
param3=0.0,
|
||||
param4=float(yaw_deg),
|
||||
param5=float(latitude) if latitude is not None else 0.0,
|
||||
param6=float(longitude) if longitude is not None else 0.0,
|
||||
param7=float(altitude_m),
|
||||
timeout_sec=timeout_sec,
|
||||
)
|
||||
|
||||
response = future.result()
|
||||
return ChangeModeResult(
|
||||
success=response.success,
|
||||
message=response.message,
|
||||
ack_result=response.ack_result,
|
||||
def arm_disarm(
|
||||
self,
|
||||
*,
|
||||
target_sysid: int,
|
||||
arm: bool,
|
||||
target_compid: int = 0,
|
||||
confirmation: int = 0,
|
||||
param2: float = 0.0,
|
||||
timeout_sec: float = DEFAULT_TIMEOUT_SEC,
|
||||
) -> CommandLongResult:
|
||||
return self._send_command_long(
|
||||
target_sysid=target_sysid,
|
||||
target_compid=target_compid,
|
||||
command=COMMAND_COMPONENT_ARM_DISARM,
|
||||
confirmation=confirmation,
|
||||
param1=1.0 if arm else 0.0,
|
||||
param2=float(param2),
|
||||
param3=0.0,
|
||||
param4=0.0,
|
||||
param5=0.0,
|
||||
param6=0.0,
|
||||
param7=0.0,
|
||||
timeout_sec=timeout_sec,
|
||||
)
|
||||
@@ -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,
|
||||
# )
|
||||
@@ -0,0 +1,48 @@
|
||||
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from fc_network_apps.navigation import PositionTargetGlobalIntClient
|
||||
import time
|
||||
|
||||
def main() -> None:
|
||||
# 目標載具 system id（依你的連線修改）
|
||||
target_sysid = 3
|
||||
|
||||
nav = PositionTargetGlobalIntClient()
|
||||
if not nav.wait_for_service(timeout_sec=5.0):
|
||||
print("Service /fc_network/vehicle/pos_global_int not available.")
|
||||
return
|
||||
|
||||
result_deg = nav.goto_position_only(
|
||||
target_sysid=target_sysid,
|
||||
latitude_deg=-35.376655,
|
||||
longitude_deg=149.157011,
|
||||
alt=200.0,
|
||||
timeout_sec=3.0,
|
||||
)
|
||||
print("=== set_position_target_global_int_deg ===")
|
||||
print(f"success : {result_deg.success}")
|
||||
print(f"ack_result: {result_deg.ack_result}")
|
||||
print(f"message : {result_deg.message}")
|
||||
print(f"r_lat_int : {result_deg.r_lat_int} r_lon_int: {result_deg.r_lon_int}")
|
||||
|
||||
|
||||
time.sleep(180)
|
||||
|
||||
result_deg = nav.goto_position_only(
|
||||
target_sysid=target_sysid,
|
||||
latitude_deg=-35.3632621,
|
||||
longitude_deg=149.1652375,
|
||||
alt=100.0,
|
||||
timeout_sec=3.0,
|
||||
)
|
||||
print("=== set_position_target_global_int_deg ===")
|
||||
print(f"success : {result_deg.success}")
|
||||
print(f"ack_result: {result_deg.ack_result}")
|
||||
print(f"message : {result_deg.message}")
|
||||
print(f"r_lat_int : {result_deg.r_lat_int} r_lon_int: {result_deg.r_lon_int}")
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -0,0 +1,64 @@
|
||||
|
||||
from fc_network_apps import CommandLongClient
|
||||
import time
|
||||
|
||||
def main():
|
||||
# Equivalent to:
|
||||
# ros2 service call /fc_network/vehicle/send_command_long ... param1:1 param2:4
|
||||
commandAPI = CommandLongClient()
|
||||
|
||||
result = commandAPI.change_mode(
|
||||
target_sysid=3,
|
||||
target_compid=0,
|
||||
base_mode=1.0,
|
||||
custom_mode=4.0,
|
||||
timeout_sec=3.0,
|
||||
)
|
||||
|
||||
print("=== change mode result ===")
|
||||
print(f"success : {result.success}")
|
||||
print(f"ack_result: {result.ack_result}")
|
||||
print(f"message : {result.message}")
|
||||
|
||||
time.sleep(3)
|
||||
|
||||
result = commandAPI.arm_disarm(
|
||||
target_sysid=3,
|
||||
target_compid=0,
|
||||
arm = True,
|
||||
)
|
||||
|
||||
print("=== change mode result ===")
|
||||
print(f"success : {result.success}")
|
||||
print(f"ack_result: {result.ack_result}")
|
||||
print(f"message : {result.message}")
|
||||
|
||||
time.sleep(1)
|
||||
|
||||
result = commandAPI.takeoff(
|
||||
target_sysid=3,
|
||||
target_compid=0,
|
||||
altitude_m = 30.0,
|
||||
)
|
||||
|
||||
print("=== change mode result ===")
|
||||
print(f"success : {result.success}")
|
||||
print(f"ack_result: {result.ack_result}")
|
||||
print(f"message : {result.message}")
|
||||
|
||||
# time.sleep(20)
|
||||
|
||||
# result = commandAPI.land(
|
||||
# target_sysid=3,
|
||||
# target_compid=0,
|
||||
# )
|
||||
|
||||
# print("=== change mode result ===")
|
||||
# print(f"success : {result.success}")
|
||||
# print(f"ack_result: {result.ack_result}")
|
||||
# print(f"message : {result.message}")
|
||||
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -24,6 +24,7 @@ python -m fc_network_adapter.tests.test_vehicleStatusPublisher
|
||||
python -m fc_network_adapter.tests.test_ringBuffer
|
||||
python -m fc_network_adapter.fc_network_adapter.mainOrchestrator
|
||||
|
||||
python -m someotherpkg.src.example_takeoff_land
|
||||
python -m someotherpkg.src.example_change_mode
|
||||
|
||||
ros2 topic list
|
||||
@@ -39,10 +40,25 @@ ros2 service call /mavlink/add_two_ints example_interfaces/srv/AddTwoInts "{a: 5
|
||||
ros2 service call /fc_network/vehicle/mav_ping fc_interfaces/srv/MavPing "{target_sysid: 3, target_compid: 0, ping_seq: 1}"
|
||||
|
||||
ros2 service call /fc_network/vehicle/send_command_long fc_interfaces/srv/MavCommandLong "{target_sysid: 3, target_compid: 0, command: 176, confirmation: 0, param1: 1, param2: 4, param3: 0,param4: 0,param5: 0,param6: 0,param7: 0, timeout_sec: 2}"
|
||||
ros2 service call /fc_network/vehicle/send_command_long fc_interfaces/srv/MavCommandLong "{target_sysid: 3, target_compid: 0, command: 176, confirmation: 0, param1: 1, param2: 4, timeout_sec: 2}"
|
||||
|
||||
MAV_CMD_DO_SET_MODE (176)
|
||||
ros2 service call /fc_network/vehicle/pos_global_int fc_interfaces/srv/MavPositionTargetGlobalInt "{target_sysid: 3, target_compid: 1, coordinate_frame: 6, type_mask: 3576, lat_int: -35376655, lon_int: 149157011, alt: 20.0, timeout_sec: 5.0}"
|
||||
|
||||
|
||||
sudo tcpdump -i lo 'udp dst port 14561' -X
|
||||
sudo tcpdump -i lo 'udp dst port 14550' -X -vv
|
||||
sudo tcpdump -i lo -X udp port 14550
|
||||
|
||||
|
||||
colcon build --packages-select fc_interfaces
|
||||
|
||||
|
||||
|
||||
-35.360150, 149.159659
|
||||
-35.376655, 149.157011
|
||||
|
||||
0b00 0000 00000 00000
|
||||
|
||||
0b00 0011 01111 11000
|
||||
0b 11 01111 11000
|
||||
0b 1111 1101 1111 1000
|
||||
Reference in New Issue
Block a user