diff --git a/README.md b/README.md index 0cbf700..95e4ebf 100644 --- a/README.md +++ b/README.md @@ -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 是執行時期的記錄檔 === diff --git a/src/fc_interfaces/CMakeLists.txt b/src/fc_interfaces/CMakeLists.txt index 909a6b8..c5c4a41 100644 --- a/src/fc_interfaces/CMakeLists.txt +++ b/src/fc_interfaces/CMakeLists.txt @@ -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() diff --git a/src/fc_interfaces/srv/MavPositionTargetGlobalInt.srv b/src/fc_interfaces/srv/MavPositionTargetGlobalInt.srv new file mode 100644 index 0000000..bd58ca0 --- /dev/null +++ b/src/fc_interfaces/srv/MavPositionTargetGlobalInt.srv @@ -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 \ No newline at end of file diff --git a/src/fc_network_adapter/fc_network_adapter/ackEnum.py b/src/fc_network_adapter/fc_network_adapter/ackEnum.py new file mode 100644 index 0000000..aefd253 --- /dev/null +++ b/src/fc_network_adapter/fc_network_adapter/ackEnum.py @@ -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 + + diff --git a/src/fc_network_adapter/fc_network_adapter/mainOrchestrator.py b/src/fc_network_adapter/fc_network_adapter/mainOrchestrator.py index 6625de1..97db4c1 100644 --- a/src/fc_network_adapter/fc_network_adapter/mainOrchestrator.py +++ b/src/fc_network_adapter/fc_network_adapter/mainOrchestrator.py @@ -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): diff --git a/src/fc_network_adapter/fc_network_adapter/mavlinkROS2Nodes.py b/src/fc_network_adapter/fc_network_adapter/mavlinkROS2Nodes.py index cebcc1b..eca0bcc 100644 --- a/src/fc_network_adapter/fc_network_adapter/mavlinkROS2Nodes.py +++ b/src/fc_network_adapter/fc_network_adapter/mavlinkROS2Nodes.py @@ -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,7 +724,96 @@ 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: 參數請求 # ═══════════════════════════════════════════════════════════════════════ @@ -790,7 +892,21 @@ class MavlinkCommandService(Node): response.success = True 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 回傳值的資訊 ''' diff --git a/src/fc_network_apps/longCommand.py b/src/fc_network_apps/longCommand.py index 6911c7c..5dbb957 100644 --- a/src/fc_network_apps/longCommand.py +++ b/src/fc_network_apps/longCommand.py @@ -1,3 +1,4 @@ +"""ROS2 client for fc_network MavCommandLong service (COMMAND_LONG 系列指令).""" from __future__ import annotations @@ -10,66 +11,181 @@ 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 change_mode( + def _send_command_long( self, *, target_sysid: int, - custom_mode: float, - target_compid: int = 0, - base_mode: float = 1.0, - confirmation: int = 0, - timeout_sec: float = DEFAULT_TIMEOUT_SEC ) -> ChangeModeResult: - + 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_DO_SET_MODE + req.command = command 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.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 ChangeModeResult( + return CommandLongResult( success=False, - message="Service call timeout or no response.", + message="Service timeout.", ack_result=-1, ) response = future.result() - return ChangeModeResult( + return CommandLongResult( success=response.success, message=response.message, ack_result=response.ack_result, - ) \ No newline at end of file + ) + + def change_mode( + self, + *, + target_sysid: int, + custom_mode: float, + target_compid: int = 0, + base_mode: float = 1.0, + confirmation: int = 0, + 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, + ) + + 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, + ) + + 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, + ) + + 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, + ) diff --git a/src/fc_network_apps/navigation.py b/src/fc_network_apps/navigation.py new file mode 100644 index 0000000..cd66374 --- /dev/null +++ b/src/fc_network_apps/navigation.py @@ -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, + # ) diff --git a/src/someotherpkg/src/example_position_goto.py b/src/someotherpkg/src/example_position_goto.py new file mode 100644 index 0000000..ab1fab3 --- /dev/null +++ b/src/someotherpkg/src/example_position_goto.py @@ -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() diff --git a/src/someotherpkg/src/example_takeoff_land.py b/src/someotherpkg/src/example_takeoff_land.py new file mode 100644 index 0000000..24c5282 --- /dev/null +++ b/src/someotherpkg/src/example_takeoff_land.py @@ -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() \ No newline at end of file diff --git a/src/unitdev02/unitdev02/devnote.txt b/src/unitdev02/unitdev02/devnote.txt index b4d7dbc..a5fec85 100644 --- a/src/unitdev02/unitdev02/devnote.txt +++ b/src/unitdev02/unitdev02/devnote.txt @@ -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 \ No newline at end of file +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 \ No newline at end of file