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

src/fc_network_apps/__init__.py

@@ -1,3 +1,14 @@
-from .changeMode import ChangeModeClient, ChangeModeResult, change_mode
+from .longCommand import CommandLongClient, ChangeModeResult
+from .changeMode import change_mode
+from .arm_disarm import arm_disarm
+from .takeoff import takeoff
+from .land import land
 
-__all__ = ["ChangeModeClient", "ChangeModeResult", "change_mode"]
+__all__ = [
+    "CommandLongClient",
+    "ChangeModeResult",
+    "change_mode",
+    "arm_disarm",
+    "takeoff",
+    "land",
+]

src/fc_network_apps/arm_disarm.py

@@ -0,0 +1,93 @@
+"""Simple wrapper for arm/disarm via fc_network ROS2 service (MAV_CMD_COMPONENT_ARM_DISARM)."""
+
+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_COMPONENT_ARM_DISARM = 400
+DEFAULT_SERVICE_NAME = "/fc_network/vehicle/send_command_long"
+DEFAULT_TIMEOUT_SEC = 2.0
+
+
+@dataclass
+class ArmDisarmResult:
+    success: bool
+    message: str
+    ack_result: int
+
+
+def arm_disarm(
+    *,
+    target_sysid: int,
+    arm: bool,
+    target_compid: int = 0,
+    confirmation: int = 0,
+    param2: float = 0.0,
+    timeout_sec: float = DEFAULT_TIMEOUT_SEC,
+    service_name: str = DEFAULT_SERVICE_NAME,
+) -> ArmDisarmResult:
+    """One-shot MAV_CMD_COMPONENT_ARM_DISARM (400) wrapper.
+
+    param1: 1.0 to arm, 0.0 to disarm.
+    param2: usually 0. Some stacks use 21196 for force-arm (ArduPilot); pass via param2 if needed.
+    """
+    rclpy.init(args=None)
+    node: Optional[Node] = None
+    try:
+        node = Node("fc_arm_disarm_client_once")
+        client = node.create_client(MavCommandLong, service_name)
+
+        if not client.wait_for_service(timeout_sec=timeout_sec):
+            return ArmDisarmResult(
+                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_COMPONENT_ARM_DISARM
+        req.confirmation = confirmation
+        req.param1 = 1.0 if arm else 0.0
+        req.param2 = float(param2)
+        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 = 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 ArmDisarmResult(
+                success=False,
+                message="Service call timeout or no response.",
+                ack_result=-1,
+            )
+
+        response = future.result()
+        return ArmDisarmResult(
+            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__":
+    r = arm_disarm(target_sysid=3, arm=True)
+    print(
+        f"arm_disarm success={r.success}, "
+        f"ack_result={r.ack_result}, message='{r.message}'"
+    )

src/fc_network_apps/changeMode.py

@@ -22,7 +22,6 @@ 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."""
@@ -31,27 +30,31 @@ class ChangeModeResult:
     message: str
     ack_result: int
 
+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:
 
-class ChangeModeClient(Node):
-    """Small ROS2 client dedicated to change flight mode."""
+    """One-shot helper for collaborators who want minimal code."""
+    rclpy.init(args=None)
+    node: Optional[Node] = None
+    try:
+        node = Node("fc_change_mode_client_once")
+        client = node.create_client(MavCommandLong, service_name)
 
-    def __init__(self, service_name: str = DEFAULT_SERVICE_NAME) -> None:
-        super().__init__("fc_change_mode_client")
-        self._client = self.create_client(MavCommandLong, service_name)
+        if not client.wait_for_service(timeout_sec=timeout_sec):
+            return ChangeModeResult(
+                success=False,
+                message=f"Service not available: {service_name}",
+                ack_result=-1,
+            )
 
-    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
@@ -66,8 +69,8 @@ class ChangeModeClient(Node):
         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)
+        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 ChangeModeResult(
                 success=False,
@@ -81,37 +84,6 @@ class ChangeModeClient(Node):
             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()

src/fc_network_apps/land.py

@@ -0,0 +1,90 @@
+"""Simple wrapper for land 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_LAND = 21
+DEFAULT_SERVICE_NAME = "/fc_network/vehicle/send_command_long"
+DEFAULT_TIMEOUT_SEC = 2.0
+
+
+@dataclass
+class LandResult:
+    success: bool
+    message: str
+    ack_result: int
+
+
+def land(
+    *,
+    target_sysid: int,
+    target_compid: int = 0,
+    yaw_deg: float = 0.0,
+    latitude: Optional[float] = None,
+    longitude: Optional[float] = None,
+    altitude_m: float = 0.0,
+    timeout_sec: float = DEFAULT_TIMEOUT_SEC,
+    service_name: str = DEFAULT_SERVICE_NAME,
+) -> LandResult:
+    """One-shot MAV_CMD_NAV_LAND wrapper."""
+    rclpy.init(args=None)
+    node: Optional[Node] = None
+    try:
+        node = Node("fc_land_client_once")
+        client = node.create_client(MavCommandLong, service_name)
+
+        if not client.wait_for_service(timeout_sec=timeout_sec):
+            return LandResult(
+                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_LAND
+        req.confirmation = 0
+        req.param1 = 0.0
+        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 LandResult(
+                success=False,
+                message="Service call timeout or no response.",
+                ack_result=-1,
+            )
+
+        response = future.result()
+        return LandResult(
+            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 = land(target_sysid=3)
+    print(
+        f"land success={result.success}, "
+        f"ack_result={result.ack_result}, message='{result.message}'"
+    )

src/fc_network_apps/longCommand.py

@@ -0,0 +1,75 @@
+
+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 CommandLongClient(Node):
+    """Small ROS2 client dedicated to change flight mode."""
+
+    def __init__(self, service_name: str = DEFAULT_SERVICE_NAME) -> None:
+        rclpy.init()
+        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,
+        )

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}'"
+    )