chiyu1468/AirTrapMine
2
0
Fork 1
Code Issues Pull Requests Packages Projects Releases Wiki Activity

1. (adding) mainOrchestrator 增加版本先驗程序

Browse Source 
2. (adding) mavlinkVehicleView 增加 read_all 快速讀取
3. (modify) 優化 longCommand 與 navigation 使其更好引用
4. (adding) 提供一個完整的 example_wholeMoving 作為範例
4. (remove) 移除 fc_network_app 重複功能
5. (modify) 修改 overview_table 部分顯示文字
This commit is contained in:
chiyu1468
2026-04-13 21:34:42 +08:00
parent 017dd4923d
commit cf213fc556
23 changed files with 392 additions and 985 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/fc_network_apps/__init__.py
+6 -10
View File
@@ -1,14 +1,10 @@
from .longCommand import CommandLongClient, ChangeModeResult
from .changeMode import change_mode
from .arm_disarm import arm_disarm
from .takeoff import takeoff
from .land import land
from .longCommand import CommandLongClient, CommandLongResult
from .navigation import PositionTargetGlobalIntClient, PositionTargetGlobalIntResult
__all__ = [
"CommandLongClient",
"ChangeModeResult",
"change_mode",
"arm_disarm",
"takeoff",
"land",
"PositionTargetGlobalIntClient",
"CommandLongResult",
"PositionTargetGlobalIntResult",
]
src/fc_network_apps/arm_disarm.py
-93
View File
@@ -1,93 +0,0 @@
"""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
-99
View File
@@ -1,99 +0,0 @@
"""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
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[Node] = None
try:
node = Node("fc_change_mode_client_once")
client = node.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,
)
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 = 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,
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,
)
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}'"
)
src/fc_network_apps/land.py
-90
View File
@@ -1,90 +0,0 @@
"""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
-4
View File
@@ -25,10 +25,6 @@ class CommandLongResult:
message: str
ack_result: int
ChangeModeResult = CommandLongResult
class CommandLongClient(Node):
"""ROS2 client : 對同一個 send_command_long service 發送各種 MAV_CMD_*。"""
src/fc_network_apps/navigation.py
-91
View File
@@ -99,7 +99,6 @@ def _echo_position_target_matches(
@dataclass
class PositionTargetGlobalIntResult:
"""對應 MavPositionTargetGlobalInt.srv 的 Response；success 依「送出與 r_* echo 是否一致」。"""
success: bool
message: str
ack_result: int
@@ -117,7 +116,6 @@ class PositionTargetGlobalIntResult:
r_yaw: float = 0.0
r_yaw_rate: float = 0.0
class PositionTargetGlobalIntClient(Node):
"""
ROS2 client：對 pos_global_int service 發送 SET_POSITION_TARGET_GLOBAL_INT (msg 86)，
@@ -230,51 +228,6 @@ class PositionTargetGlobalIntClient(Node):
r_yaw_rate=float(resp.r_yaw_rate),
)
# def set_position_target_global_int(
# self,
# *,
# target_sysid: int,
# target_compid: int = 0,
# time_boot_ms: int = 0,
# coordinate_frame: int = 6,
# type_mask: int = 0,
# lat_int: int = 0,
# lon_int: int = 0,
# alt: float = 0.0,
# vx: float = 0.0,
# vy: float = 0.0,
# vz: float = 0.0,
# afx: float = 0.0,
# afy: float = 0.0,
# afz: float = 0.0,
# yaw: float = 0.0,
# yaw_rate: float = 0.0,
# timeout_sec: float = DEFAULT_TIMEOUT_SEC,
# ) -> PositionTargetGlobalIntResult:
# """
# 送出 SET_POSITION_TARGET_GLOBAL_INT；coordinate_frame 預設 6 (MAV_FRAME_GLOBAL_INT)
# 可依飛控／任務覆寫。
# """
# return self._send_position_target_global_int(
# target_sysid=target_sysid,
# target_compid=target_compid,
# time_boot_ms=time_boot_ms,
# coordinate_frame=coordinate_frame,
# type_mask=type_mask,
# lat_int=lat_int,
# lon_int=lon_int,
# alt=alt,
# vx=vx,
# vy=vy,
# vz=vz,
# afx=afx,
# afy=afy,
# afz=afz,
# yaw=yaw,
# yaw_rate=yaw_rate,
# timeout_sec=timeout_sec,
# )
def goto_position_only(
self,
*,
@@ -307,47 +260,3 @@ class PositionTargetGlobalIntClient(Node):
yaw_rate=0,
timeout_sec=timeout_sec,
)
# def set_position_target_global_int_deg(
# self,
# *,
# target_sysid: int,
# latitude_deg: float,
# longitude_deg: float,
# target_compid: int = 0,
# time_boot_ms: int = 0,
# coordinate_frame: int = MAV_FRAME_GLOBAL_RELATIVE_ALT_INT,
# type_mask: int = 0,
# alt: float = 0.0,
# vx: float = 0.0,
# vy: float = 0.0,
# vz: float = 0.0,
# afx: float = 0.0,
# afy: float = 0.0,
# afz: float = 0.0,
# yaw: float = 0.0,
# yaw_rate: float = 0.0,
# timeout_sec: float = DEFAULT_TIMEOUT_SEC,
# ) -> PositionTargetGlobalIntResult:
# """以度為單位的緯經，轉成 MAVLink 的 lat_int / lon_int (1e7 * deg)。"""
# lat_int = int(round(latitude_deg * 1e7))
# lon_int = int(round(longitude_deg * 1e7))
# return self.set_position_target_global_int(
# target_sysid=target_sysid,
# target_compid=target_compid,
# time_boot_ms=time_boot_ms,
# coordinate_frame=coordinate_frame,
# type_mask=type_mask,
# lat_int=lat_int,
# lon_int=lon_int,
# alt=alt,
# vx=vx,
# vy=vy,
# vz=vz,
# afx=afx,
# afy=afy,
# afz=afz,
# yaw=yaw,
# yaw_rate=yaw_rate,
# timeout_sec=timeout_sec,
# )
src/fc_network_apps/takeoff.py
-91
View File
@@ -1,91 +0,0 @@
"""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}'"
)