diff --git a/src/fc_network_adapter/tests/example_mavlink_client.py b/src/fc_network_adapter/tests/example_mavlink_client.py new file mode 100644 index 0000000..9f59c33 --- /dev/null +++ b/src/fc_network_adapter/tests/example_mavlink_client.py @@ -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. 易於測試和維護 + +═══════════════════════════════════════════════════════════════════════════ +""" diff --git a/src/fc_network_apps/__init__.py b/src/fc_network_apps/__init__.py new file mode 100644 index 0000000..a1a700f --- /dev/null +++ b/src/fc_network_apps/__init__.py @@ -0,0 +1,3 @@ +from .changeMode import ChangeModeClient, ChangeModeResult, change_mode + +__all__ = ["ChangeModeClient", "ChangeModeResult", "change_mode"] diff --git a/src/fc_network_apps/changeMode.py b/src/fc_network_apps/changeMode.py new file mode 100644 index 0000000..35a67a6 --- /dev/null +++ b/src/fc_network_apps/changeMode.py @@ -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}'" + ) \ No newline at end of file diff --git a/src/someotherpkg/src/example_change_mode.py b/src/someotherpkg/src/example_change_mode.py new file mode 100644 index 0000000..108244a --- /dev/null +++ b/src/someotherpkg/src/example_change_mode.py @@ -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() diff --git a/src/unitdev02/unitdev02/devnote.txt b/src/unitdev02/unitdev02/devnote.txt index 4ec0660..9b05455 100644 --- a/src/unitdev02/unitdev02/devnote.txt +++ b/src/unitdev02/unitdev02/devnote.txt @@ -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