From 44d53f51fbd2b58fd6cc210c5002b751a30afa07 Mon Sep 17 00:00:00 2001 From: Chiyu Chen Date: Tue, 31 Mar 2026 15:54:07 +0800 Subject: [PATCH] =?UTF-8?q?(tested)=201.=20=E5=A2=9E=E5=8A=A0=E5=90=84?= =?UTF-8?q?=E7=A8=AE=20command=20long=20=E7=9A=84=E5=8A=9F=E8=83=BD?= =?UTF-8?q?=E8=88=87=E4=BD=BF=E7=94=A8=E7=AF=84=E4=BE=8B=202.=20=E5=88=AA?= =?UTF-8?q?=E9=99=A4=E7=84=A1=E7=94=A8=E7=9A=84=20mavlinkPublish.py=203.?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- README.md | 4 + .../fc_network_adapter/mainOrchestrator.py | 2 +- .../fc_network_adapter/mavlinkPublish.py | 212 ------------------ src/fc_network_apps/__init__.py | 15 +- src/fc_network_apps/arm_disarm.py | 93 ++++++++ src/fc_network_apps/changeMode.py | 76 ++----- src/fc_network_apps/land.py | 90 ++++++++ src/fc_network_apps/longCommand.py | 75 +++++++ src/fc_network_apps/takeoff.py | 91 ++++++++ src/someotherpkg/src/example2_change_mode.py | 55 +++++ src/someotherpkg/src/example_arm_disarm.py | 30 +++ src/someotherpkg/src/example_land.py | 27 +++ src/someotherpkg/src/example_takeoff.py | 28 +++ src/unitdev02/unitdev02/devnote.txt | 2 + 14 files changed, 533 insertions(+), 267 deletions(-) delete mode 100644 src/fc_network_adapter/fc_network_adapter/mavlinkPublish.py create mode 100644 src/fc_network_apps/arm_disarm.py create mode 100644 src/fc_network_apps/land.py create mode 100644 src/fc_network_apps/longCommand.py create mode 100644 src/fc_network_apps/takeoff.py create mode 100644 src/someotherpkg/src/example2_change_mode.py create mode 100644 src/someotherpkg/src/example_arm_disarm.py create mode 100644 src/someotherpkg/src/example_land.py create mode 100644 src/someotherpkg/src/example_takeoff.py diff --git a/README.md b/README.md index 4cc0bcc..d7de3e4 100644 --- a/README.md +++ b/README.md @@ -64,6 +64,10 @@ Package 簡述 構築 mavlink 封包 處理無線模組的通訊格式 (XBee) --同時處理與 Gazebo 的 ardupilot_plugin 溝通的 FDM/JSON 訊息 (移除)-- +3. fc_interfaces + 自定義的介面檔 +4. fc_network_apps + 與 fc_network_adapter 銜接做高階功能包裝的應用小程式 利於開發GUI或其他應用 N. logs 是執行時期的記錄檔 diff --git a/src/fc_network_adapter/fc_network_adapter/mainOrchestrator.py b/src/fc_network_adapter/fc_network_adapter/mainOrchestrator.py index 89d88fb..6625de1 100644 --- a/src/fc_network_adapter/fc_network_adapter/mainOrchestrator.py +++ b/src/fc_network_adapter/fc_network_adapter/mainOrchestrator.py @@ -29,7 +29,7 @@ from .utils import acquireSerial, acquirePort from .utils.acquirePort import find_available_port logger = setup_logger(os.path.basename(__file__)) -VERSION_NO = "v0.59" +VERSION_NO = "v0.60" class PanelState: def __init__(self): diff --git a/src/fc_network_adapter/fc_network_adapter/mavlinkPublish.py b/src/fc_network_adapter/fc_network_adapter/mavlinkPublish.py deleted file mode 100644 index 5dc3ca1..0000000 --- a/src/fc_network_adapter/fc_network_adapter/mavlinkPublish.py +++ /dev/null @@ -1,212 +0,0 @@ - -''' -這個檔案只有一個 class -是作為 mavlinkObject.py 中 mavlink_analyzer 類別的功能衍生 - -主要概念是將 "離散的" mavlink 參數轉換成 ROS topic -包含了創建 publisher 和 以及包裝並丟到 ros2 topic 的 packEmit - -publisher topic name 命名規則為 <前綴詞>/s/<具體 topic> -''' - -import os - -# ROS2 的 import -import std_msgs.msg -import sensor_msgs.msg -import geometry_msgs.msg -import mavros_msgs.msg - -import math - -# 自定義的 import -from .utils import setup_logger - -logger = setup_logger(os.path.basename(__file__)) - -class mavlink_publisher(): - - prefix_path = 'MavLinkBus' - - def create_flightMode(self, sysid, component_obj): - # target topic name # 請跟這個 method 的名稱保持一致 - target_topic = 'flightMode' - - # 這邊要檢查 flight_mode 是否存在 - try: - _ = component_obj.emitParams['flightMode_mode'] - except KeyError: - # 這個 component id 還不存在 - logger.warning('System ID : {} This Component ID : {} Did not init yet'.format(component_obj['sysid'], component_obj['compid'])) - return False - - # 若存在則 建立 publisher object 並回傳 true - topic_name = '{}/s{}/{}'.format(self.prefix_path, sysid, target_topic) - publisher_ = self.create_publisher(std_msgs.msg.String, topic_name, 1) - component_obj.publishers[target_topic] = [publisher_, self.packEmit_flightMode] - - return True - - def packEmit_flightMode(cls, emitParams, publisher): - msg_str = emitParams['flightMode_mode'] - msg = std_msgs.msg.String() - msg.data = msg_str - publisher.publish(msg) - pass - - # ↓↓↓↓↓↓↓↓↓↓↓↓ 處理不同 ros2 topic 訊息 請放在這裡 ↓↓↓↓↓↓↓↓↓↓↓↓ - def euler_to_quaternion(cls,roll, pitch, yaw): - qx = math.sin(roll/2) * math.cos(pitch/2) * math.cos(yaw/2) - math.cos(roll/2) * math.sin(pitch/2) * math.sin(yaw/2) - qy = math.cos(roll/2) * math.sin(pitch/2) * math.cos(yaw/2) + math.sin(roll/2) * math.cos(pitch/2) * math.sin(yaw/2) - qz = math.cos(roll/2) * math.cos(pitch/2) * math.sin(yaw/2) - math.sin(roll/2) * math.sin(pitch/2) * math.cos(yaw/2) - qw = math.cos(roll/2) * math.cos(pitch/2) * math.cos(yaw/2) + math.sin(roll/2) * math.sin(pitch/2) * math.sin(yaw/2) - return [qx, qy, qz, qw] - - def create_attitude(self, sysid, component_obj): - target_topic = 'attitude' - - try: - _ = component_obj.emitParams['attitude'] - except KeyError: - logger.warning('System ID : {} This Component ID : {} Did not init yet'.format(component_obj['sysid'], component_obj['compid'])) - return False - - topic_name = '{}/s{}/{}'.format(self.prefix_path, sysid, target_topic) - publisher_ = self.create_publisher(sensor_msgs.msg.Imu, topic_name, 1) - component_obj.publishers[target_topic] = [publisher_, self.packEmit_attitude] - - def packEmit_attitude(self, emitParams, publisher): - mav_msg = emitParams['attitude'] - msg = sensor_msgs.msg.Imu() - x, y, z, w = self.euler_to_quaternion(mav_msg.roll, mav_msg.pitch, mav_msg.yaw) - msg.orientation.x = x - msg.orientation.y = y - msg.orientation.z = z - msg.orientation.w = w - msg.angular_velocity.x = mav_msg.rollspeed - msg.angular_velocity.y = mav_msg.pitchspeed - msg.angular_velocity.z = mav_msg.yawspeed - publisher.publish(msg) - pass - - def create_local_position_pose(self, sysid, component_obj): - target_topic = 'local_position/pose' - try: - _ = component_obj.emitParams['local_position'] - except KeyError: - logger.warning('System ID : {} This Component ID : {} Did not init yet'.format(component_obj['sysid'], component_obj['compid'])) - return False - topic_name = '{}/s{}/{}'.format(self.prefix_path, sysid, target_topic) - publisher_ = self.create_publisher(geometry_msgs.msg.Point, topic_name, 1) - component_obj.publishers[target_topic] = [publisher_, self.packEmit_local_pose] - - def packEmit_local_pose(cls, emitParams, publisher): - mav_msg = emitParams['local_position'] - msg = geometry_msgs.msg.Point() - msg.x = mav_msg.x - msg.y = mav_msg.y - msg.z = mav_msg.z - publisher.publish(msg) - pass - - def create_local_position_velocity(self, sysid, component_obj): - target_topic = 'local_position/velocity' - try: - _ = component_obj.emitParams['local_position'] - except KeyError: - logger.warning('System ID : {} This Component ID : {} Did not init yet'.format(component_obj['sysid'], component_obj['compid'])) - return False - topic_name = '{}/s{}/{}'.format(self.prefix_path, sysid, target_topic) - publisher_ = self.create_publisher(geometry_msgs.msg.Vector3, topic_name, 1) - component_obj.publishers[target_topic] = [publisher_, self.packEmit_local_vel] - - def packEmit_local_vel(cls, emitParams, publisher): - mav_msg = emitParams['local_position'] - msg = geometry_msgs.msg.Vector3() - msg.x = mav_msg.vx - msg.y = mav_msg.vy - msg.z = mav_msg.vz - publisher.publish(msg) - pass - - def create_global_global(self, sysid, component_obj): - target_topic = 'global_position/global' - try: - _ = component_obj.emitParams['global_position'] - except KeyError: - logger.warning('System ID : {} This Component ID : {} Did not init yet'.format(component_obj['sysid'], component_obj['compid'])) - return False - topic_name = '{}/s{}/{}'.format(self.prefix_path, sysid, target_topic) - publisher_ = self.create_publisher(sensor_msgs.msg.NavSatFix, topic_name, 1) - component_obj.publishers[target_topic] = [publisher_, self.packEmit_global_global] - - def packEmit_global_global(cls, emitParams, publisher): - mav_msg = emitParams['global_position'] - msg = sensor_msgs.msg.NavSatFix() - msg.latitude = mav_msg.lat/1e7 - msg.longitude = mav_msg.lon/1e7 - msg.altitude = mav_msg.alt/1e3 - publisher.publish(msg) - pass - - def create_global_rel(self, sysid, component_obj): - target_topic = 'global_position/rel_alt' - try: - _ = component_obj.emitParams['global_position'] - except KeyError: - logger.warning('System ID : {} This Component ID : {} Did not init yet'.format(component_obj['sysid'], component_obj['compid'])) - return False - topic_name = '{}/s{}/{}'.format(self.prefix_path, sysid, target_topic) - publisher_ = self.create_publisher(std_msgs.msg.Float64, topic_name, 1) - component_obj.publishers[target_topic] = [publisher_, self.packEmit_global_rel] - - def packEmit_global_rel(cls, emitParams, publisher): - mav_msg = emitParams['global_position'] - msg = std_msgs.msg.Float64() - msg.data = float(mav_msg.relative_alt/1e3) - publisher.publish(msg) - pass - - def create_vfr_hud(self, sysid, component_obj): - target_topic = 'vfr_hud' - try: - _ = component_obj.emitParams['vfr_hud'] - except KeyError: - logger.warning('System ID : {} This Component ID : {} Did not init yet'.format(component_obj['sysid'], component_obj['compid'])) - return False - topic_name = '{}/s{}/{}'.format(self.prefix_path, sysid, target_topic) - publisher_ = self.create_publisher(mavros_msgs.msg.VfrHud, topic_name, 1) - component_obj.publishers[target_topic] = [publisher_, self.packEmit_vfr_hud] - - def packEmit_vfr_hud(cls, emitParams, publisher): - mav_msg = emitParams['vfr_hud'] - msg = mavros_msgs.msg.VfrHud() - msg.airspeed = mav_msg.airspeed - msg.groundspeed = mav_msg.groundspeed - msg.heading = mav_msg.heading - msg.throttle = float(mav_msg.throttle) - msg.altitude = mav_msg.alt - msg.climb = mav_msg.climb - publisher.publish(msg) - pass - - def create_battery(self, sysid, component_obj): - target_topic = 'battery' - try: - _ = component_obj.emitParams['battery'] - except KeyError: - logger.warning('System ID : {} This Component ID : {} Did not init yet'.format(component_obj['sysid'], component_obj['compid'])) - return False - topic_name = '{}/s{}/{}'.format(self.prefix_path, sysid, target_topic) - publisher_ = self.create_publisher(sensor_msgs.msg.BatteryState, topic_name, 1) - component_obj.publishers[target_topic] = [publisher_, self.packEmit_battery] - - def packEmit_battery(cls, emitParams, publisher): - mav_msg = emitParams['battery'] - msg = sensor_msgs.msg.BatteryState() - msg.voltage = mav_msg.voltages[0]/1e3 - publisher.publish(msg) - pass - - - # ↑↑↑↑↑↑↑↑↑↑↑↑ 處理不同 ros2 topic 訊息 請放在這裡 ↑↑↑↑↑↑↑↑↑↑↑↑ \ No newline at end of file diff --git a/src/fc_network_apps/__init__.py b/src/fc_network_apps/__init__.py index a1a700f..d619718 100644 --- a/src/fc_network_apps/__init__.py +++ b/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", +] diff --git a/src/fc_network_apps/arm_disarm.py b/src/fc_network_apps/arm_disarm.py new file mode 100644 index 0000000..21f8386 --- /dev/null +++ b/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}'" + ) diff --git a/src/fc_network_apps/changeMode.py b/src/fc_network_apps/changeMode.py index 35a67a6..aa98230 100644 --- a/src/fc_network_apps/changeMode.py +++ b/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.""" - - def __init__(self, service_name: str = DEFAULT_SERVICE_NAME) -> None: - super().__init__("fc_change_mode_client") - self._client = self.create_client(MavCommandLong, service_name) + """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 wait_for_service(self, timeout_sec: float = 3.0) -> bool: - return self._client.wait_for_service(timeout_sec=timeout_sec) + 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 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() diff --git a/src/fc_network_apps/land.py b/src/fc_network_apps/land.py new file mode 100644 index 0000000..d21f381 --- /dev/null +++ b/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}'" + ) diff --git a/src/fc_network_apps/longCommand.py b/src/fc_network_apps/longCommand.py new file mode 100644 index 0000000..6911c7c --- /dev/null +++ b/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, + ) \ No newline at end of file diff --git a/src/fc_network_apps/takeoff.py b/src/fc_network_apps/takeoff.py new file mode 100644 index 0000000..47754b7 --- /dev/null +++ b/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}'" + ) diff --git a/src/someotherpkg/src/example2_change_mode.py b/src/someotherpkg/src/example2_change_mode.py new file mode 100644 index 0000000..37ef478 --- /dev/null +++ b/src/someotherpkg/src/example2_change_mode.py @@ -0,0 +1,55 @@ + + +from fc_network_apps import CommandLongClient +import time + +def main(): + # Equivalent to: + # ros2 service call /fc_network/vehicle/send_command_long ... param1:1 param2:4 + commandAPI = CommandLongClient() + result = commandAPI.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}") + + time.sleep(1) + + result = commandAPI.change_mode( + target_sysid=3, + target_compid=0, + base_mode=1.0, + custom_mode=3.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}") + + time.sleep(1) + + result = commandAPI.change_mode( + target_sysid=3, + target_compid=0, + base_mode=1.0, + custom_mode=5.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() \ No newline at end of file diff --git a/src/someotherpkg/src/example_arm_disarm.py b/src/someotherpkg/src/example_arm_disarm.py new file mode 100644 index 0000000..8b55a27 --- /dev/null +++ b/src/someotherpkg/src/example_arm_disarm.py @@ -0,0 +1,30 @@ +"""Usage example for arm/disarm helper. + +Run from repo root with module mode: + python -m someotherpkg.src.example_arm_disarm +""" + +from fc_network_apps import arm_disarm + + +def main() -> None: + # MAV_CMD_COMPONENT_ARM_DISARM (command=400) + # param1: 1 = arm, 0 = disarm + result = arm_disarm( + target_sysid=3, + target_compid=0, + arm=True, + timeout_sec=2.0, + ) + + print("=== arm result ===") + print(f"success : {result.success}") + print(f"ack_result: {result.ack_result}") + print(f"message : {result.message}") + + # To disarm instead: + # result = arm_disarm(target_sysid=3, target_compid=0, arm=False, timeout_sec=2.0) + + +if __name__ == "__main__": + main() diff --git a/src/someotherpkg/src/example_land.py b/src/someotherpkg/src/example_land.py new file mode 100644 index 0000000..460ca5a --- /dev/null +++ b/src/someotherpkg/src/example_land.py @@ -0,0 +1,27 @@ +"""Usage example for land helper. + +Run from repo root with module mode: + python -m someotherpkg.src.example_land +""" + +from fc_network_apps import land + + +def main() -> None: + # MAV_CMD_NAV_LAND (command=21) + # This example asks vehicle sysid=3 to land. + result = land( + target_sysid=3, + target_compid=0, + yaw_deg=0.0, + timeout_sec=2.0, + ) + + print("=== land 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/someotherpkg/src/example_takeoff.py b/src/someotherpkg/src/example_takeoff.py new file mode 100644 index 0000000..3d0fa54 --- /dev/null +++ b/src/someotherpkg/src/example_takeoff.py @@ -0,0 +1,28 @@ +"""Usage example for takeoff helper. + +Run from repo root with module mode: + python -m someotherpkg.src.example_takeoff +""" + +from fc_network_apps import takeoff + + +def main() -> None: + # MAV_CMD_NAV_TAKEOFF (command=22) + # This example asks vehicle sysid=3 to take off to 10 meters. + result = takeoff( + target_sysid=3, + target_compid=0, + altitude_m=10.0, + yaw_deg=0.0, + timeout_sec=2.0, + ) + + print("=== takeoff 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 9b05455..b4d7dbc 100644 --- a/src/unitdev02/unitdev02/devnote.txt +++ b/src/unitdev02/unitdev02/devnote.txt @@ -24,6 +24,8 @@ python -m fc_network_adapter.tests.test_vehicleStatusPublisher python -m fc_network_adapter.tests.test_ringBuffer python -m fc_network_adapter.fc_network_adapter.mainOrchestrator +python -m someotherpkg.src.example_change_mode + ros2 topic list ros2 topic echo