|
|
"""
|
|
|
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. 易於測試和維護
|
|
|
|
|
|
═══════════════════════════════════════════════════════════════════════════
|
|
|
"""
|