(tested) 將 commandLong 的 ros2 service 包裝為一個函式庫 changeMode 並附上一個臨時範例

2026-03-27 16:00:45 +08:00
parent 60d6eba8cd
commit e4585134cc
5 changed files with 344 additions and 1 deletions
@@ -0,0 +1,180 @@
+"""
+MavlinkCommandService 使用範例
+
+展示如何從其他 ROS2 節點調用 MAVLink 指令服務
+"""
+
+import rclpy
+from rclpy.node import Node
+from std_srvs.srv import Trigger
+import time
+
+
+class MavlinkClientExample(Node):
+    """
+    範例：MAVLink Service Client
+    
+    這個節點展示如何調用 MavlinkCommandService 提供的服務
+    """
+    
+    def __init__(self):
+        super().__init__('mavlink_client_example')
+        
+        # 創建 service client
+        self.client = self.create_client(
+            Trigger,
+            '/mavlink/send_command_long'
+        )
+        
+        # 等待服務可用
+        self.get_logger().info('等待 service 可用...')
+        self.client.wait_for_service()
+        self.get_logger().info('Service 已連接！')
+    
+    def send_arm_command(self):
+        """
+        範例：發送 ARM 指令
+        
+        實際使用時應該發送：
+        - message_id = 76 (COMMAND_LONG)
+        - command = 400 (MAV_CMD_COMPONENT_ARM_DISARM)
+        - param1 = 1 (ARM)
+        """
+        self.get_logger().info('發送 ARM 指令...')
+        
+        request = Trigger.Request()
+        # TODO: 實際使用時應該是自定義的 SendCommandLong.Request
+        # request.target_sysid = 1
+        # request.target_compid = 1
+        # request.command = 400  # MAV_CMD_COMPONENT_ARM_DISARM
+        # request.param1 = 1.0   # 1 = ARM, 0 = DISARM
+        # request.param2 = 0.0
+        # ... param3-7
+        # request.timeout = 3.0
+        
+        # 異步調用
+        future = self.client.call_async(request)
+        
+        # 等待結果
+        rclpy.spin_until_future_complete(self, future)
+        
+        if future.done():
+            response = future.result()
+            if response.success:
+                self.get_logger().info('✓ ARM 指令發送成功')
+            else:
+                self.get_logger().error(f'✗ ARM 指令失敗: {response.message}')
+        else:
+            self.get_logger().error('✗ Service 調用超時')
+
+
+def main():
+    """主函數"""
+    rclpy.init()
+    
+    # 創建客戶端節點
+    client = MavlinkClientExample()
+    
+    # 發送指令
+    client.send_arm_command()
+    
+    # 清理
+    client.destroy_node()
+    rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()
+
+
+"""
+═══════════════════════════════════════════════════════════════════════════
+進階使用範例：高層應用程式
+
+假設我們要創建一個 "任務控制器"，通過 MavlinkCommandService 來控制載具：
+
+```python
+class MissionController(Node):
+    def __init__(self):
+        super().__init__('mission_controller')
+        
+        # 創建各種 service clients
+        self.client_arm = self.create_client(
+            SendCommandLong, '/mavlink/send_command_long')
+        self.client_mode = self.create_client(
+            SendCommandLong, '/mavlink/send_command_long')
+        self.client_takeoff = self.create_client(
+            SendCommandLong, '/mavlink/send_command_long')
+        self.client_goto = self.create_client(
+            SendCommandInt, '/mavlink/send_command_int')
+    
+    def arm_vehicle(self, sysid=1):
+        \"\"\"解鎖載具\"\"\"
+        request = SendCommandLong.Request()
+        request.target_sysid = sysid
+        request.target_compid = 1
+        request.command = 400  # MAV_CMD_COMPONENT_ARM_DISARM
+        request.param1 = 1.0   # ARM
+        
+        future = self.client_arm.call_async(request)
+        # 處理回應...
+    
+    def set_mode(self, sysid=1, mode='GUIDED'):
+        \"\"\"設置飛行模式\"\"\"
+        # 實現模式切換邏輯...
+        pass
+    
+    def takeoff(self, sysid=1, altitude=10.0):
+        \"\"\"起飛\"\"\"
+        request = SendCommandLong.Request()
+        request.target_sysid = sysid
+        request.target_compid = 1
+        request.command = 22  # MAV_CMD_NAV_TAKEOFF
+        request.param7 = altitude
+        
+        future = self.client_takeoff.call_async(request)
+        # 處理回應...
+    
+    def goto_position(self, sysid=1, lat=0, lon=0, alt=10):
+        \"\"\"前往指定位置\"\"\"
+        request = SendCommandInt.Request()
+        request.target_sysid = sysid
+        request.target_compid = 1
+        request.command = 192  # MAV_CMD_DO_REPOSITION
+        request.x = int(lat * 1e7)
+        request.y = int(lon * 1e7)
+        request.z = alt
+        
+        future = self.client_goto.call_async(request)
+        # 處理回應...
+    
+    def execute_mission(self):
+        \"\"\"執行完整任務\"\"\"
+        # 1. 解鎖
+        self.arm_vehicle()
+        time.sleep(1)
+        
+        # 2. 切換到 GUIDED 模式
+        self.set_mode(mode='GUIDED')
+        time.sleep(1)
+        
+        # 3. 起飛到 10 公尺
+        self.takeoff(altitude=10.0)
+        time.sleep(10)
+        
+        # 4. 前往目標點
+        self.goto_position(lat=25.033, lon=121.565, alt=10)
+        time.sleep(30)
+        
+        # 5. 返回並降落
+        # ...
+```
+
+這樣的設計讓高層應用可以：
+1. 完全不需要知道 MAVLink 協議細節
+2. 通過 ROS2 service 與載具互動
+3. 模組化開發不同功能
+4. 易於測試和維護
+
+═══════════════════════════════════════════════════════════════════════════
+"""
@@ -0,0 +1,3 @@
+from .changeMode import ChangeModeClient, ChangeModeResult, change_mode
+
+__all__ = ["ChangeModeClient", "ChangeModeResult", "change_mode"]
@@ -0,0 +1,127 @@
+"""Simple wrapper for mode change via fc_network ROS2 service.
+
+Reference CLI command:
+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}"
+"""
+
+from __future__ import annotations
+
+from dataclasses import dataclass
+from typing import Optional
+
+import rclpy
+from rclpy.node import Node
+
+from fc_interfaces.srv import MavCommandLong
+
+COMMAND_DO_SET_MODE = 176
+DEFAULT_SERVICE_NAME = "/fc_network/vehicle/send_command_long"
+DEFAULT_TIMEOUT_SEC = 2.0
+
+
+@dataclass
+class ChangeModeResult:
+    """Return value for mode change requests."""
+
+    success: bool
+    message: str
+    ack_result: int
+
+
+class ChangeModeClient(Node):
+    """Small ROS2 client dedicated to change flight mode."""
+
+    def __init__(self, service_name: str = DEFAULT_SERVICE_NAME) -> None:
+        super().__init__("fc_change_mode_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(
+        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:
+        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)
+
+        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,
+            )
+
+        response = future.result()
+        return ChangeModeResult(
+            success=response.success,
+            message=response.message,
+            ack_result=response.ack_result,
+        )
+
+
+def change_mode(
+    *,
+    target_sysid: int,
+    custom_mode: float,
+    target_compid: int = 0,
+    base_mode: float = 1.0,
+    confirmation: int = 0,
+    timeout_sec: float = DEFAULT_TIMEOUT_SEC,
+    service_name: str = DEFAULT_SERVICE_NAME,
+) -> ChangeModeResult:
+    """One-shot helper for collaborators who want minimal code."""
+    rclpy.init(args=None)
+    node: Optional[ChangeModeClient] = None
+    try:
+        node = ChangeModeClient(service_name=service_name)
+        if not node.wait_for_service(timeout_sec=timeout_sec):
+            return ChangeModeResult(
+                success=False,
+                message=f"Service not available: {service_name}",
+                ack_result=-1,
+            )
+        return node.change_mode(
+            target_sysid=target_sysid,
+            target_compid=target_compid,
+            custom_mode=custom_mode,
+            base_mode=base_mode,
+            confirmation=confirmation,
+            timeout_sec=timeout_sec,
+        )
+    finally:
+        if node is not None:
+            node.destroy_node()
+        rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    # Example values are aligned with your terminal command.
+    result = change_mode(target_sysid=3, custom_mode=4)
+    print(
+        f"change_mode success={result.success}, "
+        f"ack_result={result.ack_result}, message='{result.message}'"
+    )
@@ -0,0 +1,29 @@
+"""Usage example for collaborators.
+
+Run after sourcing your ROS2 workspace:
+    source install/local_setup.bash
+    python src/fc_network_apps/example_change_mode.py
+"""
+
+from fc_network_apps import change_mode
+
+
+def main() -> None:
+    # Equivalent to:
+    # ros2 service call /fc_network/vehicle/send_command_long ... param1:1 param2:4
+    result = change_mode(
+        target_sysid=3,
+        target_compid=0,
+        base_mode=1.0,
+        custom_mode=4.0,
+        timeout_sec=2.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()
@@ -32,9 +32,13 @@ ros2 topic echo

 mavproxy.py --master=tcp:127.0.0.1:5790 --out=udp:127.0.0.1:14560

-ros2 service call /fc_network/vehicle/mav_ping fc_interfaces/srv/MavPing "{target_sysid: 3, target_compid: 0, ping_seq: 1}"

 ros2 service call /mavlink/add_two_ints example_interfaces/srv/AddTwoInts "{a: 5,b: 8}"
+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}"
+
+MAV_CMD_DO_SET_MODE (176)


 sudo tcpdump -i lo 'udp dst port 14561' -X