Merge branch 'chiyu'

1. 初步完善 ros2 service 功能 包含解鎖起飛降落與 goto
2. 為了整合到 GUI 去使用

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 是執行時期的記錄檔
 
 ===

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()

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     

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
+
+

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):

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,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 回傳值的資訊
 
 '''
 

src/fc_network_apps/longCommand.py

@@ -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:
+class CommandLongResult:
-    """Return value for mode change requests."""
-
     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()
+        if not rclpy.ok():
-        super().__init__("fc_change_mode_client")
+            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()
+    def takeoff(
-        req.target_sysid = target_sysid
+        self,
-        req.target_compid = target_compid
+        *,
-        req.command = COMMAND_DO_SET_MODE
+        target_sysid: int,
-        req.confirmation = confirmation
+        altitude_m: float,
-        req.param1 = float(base_mode)
+        target_compid: int = 0,
-        req.param2 = float(custom_mode)
+        min_pitch_deg: float = 0.0,
-        req.param3 = 0.0
+        yaw_deg: float = 0.0,
-        req.param4 = 0.0
+        latitude: Optional[float] = None,
-        req.param5 = 0.0
+        longitude: Optional[float] = None,
-        req.param6 = 0.0
+        timeout_sec: float = DEFAULT_TIMEOUT_SEC,
-        req.param7 = 0.0
+    ) -> CommandLongResult:
-        req.timeout_sec = float(timeout_sec)
+        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)
+    def land(
-        rclpy.spin_until_future_complete(self, future, timeout_sec=timeout_sec + 1.0)
+        self,
-        if not future.done() or future.result() is None:
+        *,
-            return ChangeModeResult(
+        target_sysid: int,
-                success=False,
+        target_compid: int = 0,
-                message="Service call timeout or no response.",
+        yaw_deg: float = 0.0,
-                ack_result=-1,
+        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()
+    def arm_disarm(
-        return ChangeModeResult(
+        self,
-            success=response.success,
+        *,
-            message=response.message,
+        target_sysid: int,
-            ack_result=response.ack_result,
+        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,
         )

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,
+    #     )

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()

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()

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
+
+
+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