(tested) 1. 增加各種 command long 的功能與使用範例 2. 刪除無用的 mavlinkPublish.py 3.

src/fc_network_apps/takeoff.py

@@ -0,0 +1,91 @@
+"""Simple wrapper for takeoff via fc_network ROS2 service."""
+
+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_NAV_TAKEOFF = 22
+DEFAULT_SERVICE_NAME = "/fc_network/vehicle/send_command_long"
+DEFAULT_TIMEOUT_SEC = 2.0
+
+
+@dataclass
+class TakeoffResult:
+    success: bool
+    message: str
+    ack_result: int
+
+
+def takeoff(
+    *,
+    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,
+    service_name: str = DEFAULT_SERVICE_NAME,
+) -> TakeoffResult:
+    """One-shot MAV_CMD_NAV_TAKEOFF wrapper."""
+    rclpy.init(args=None)
+    node: Optional[Node] = None
+    try:
+        node = Node("fc_takeoff_client_once")
+        client = node.create_client(MavCommandLong, service_name)
+
+        if not client.wait_for_service(timeout_sec=timeout_sec):
+            return TakeoffResult(
+                success=False,
+                message=f"Service not available: {service_name}",
+                ack_result=-1,
+            )
+
+        req = MavCommandLong.Request()
+        req.target_sysid = target_sysid
+        req.target_compid = target_compid
+        req.command = COMMAND_NAV_TAKEOFF
+        req.confirmation = 0
+        req.param1 = float(min_pitch_deg)
+        req.param2 = 0.0
+        req.param3 = 0.0
+        req.param4 = float(yaw_deg)
+        req.param5 = float(latitude) if latitude is not None else 0.0
+        req.param6 = float(longitude) if longitude is not None else 0.0
+        req.param7 = float(altitude_m)
+        req.timeout_sec = float(timeout_sec)
+
+        future = client.call_async(req)
+        rclpy.spin_until_future_complete(node, future, timeout_sec=timeout_sec + 1.0)
+        if not future.done() or future.result() is None:
+            return TakeoffResult(
+                success=False,
+                message="Service call timeout or no response.",
+                ack_result=-1,
+            )
+
+        response = future.result()
+        return TakeoffResult(
+            success=response.success,
+            message=response.message,
+            ack_result=response.ack_result,
+        )
+    finally:
+        if node is not None:
+            node.destroy_node()
+        rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    result = takeoff(target_sysid=3, altitude_m=10.0)
+    print(
+        f"takeoff success={result.success}, "
+        f"ack_result={result.ack_result}, message='{result.message}'"
+    )