Compare commits
1 Commits
2c38376f9c
...
f2bb4679c2
| Author | SHA1 | Date |
|---|---|---|
 ken910606
|
f2bb4679c2 | 1 month ago |
@ -1,10 +1,14 @@
|
|||||||
from .longCommand import CommandLongClient, CommandLongResult
|
from .longCommand import CommandLongClient, ChangeModeResult
|
||||||
from .navigation import PositionTargetGlobalIntClient, PositionTargetGlobalIntResult
|
from .changeMode import change_mode
|
||||||
|
from .arm_disarm import arm_disarm
|
||||||
|
from .takeoff import takeoff
|
||||||
|
from .land import land
|
||||||
|
|
||||||
__all__ = [
|
__all__ = [
|
||||||
"CommandLongClient",
|
"CommandLongClient",
|
||||||
"PositionTargetGlobalIntClient",
|
"ChangeModeResult",
|
||||||
"CommandLongResult",
|
"change_mode",
|
||||||
"PositionTargetGlobalIntResult",
|
"arm_disarm",
|
||||||
|
"takeoff",
|
||||||
|
"land",
|
||||||
]
|
]
|
||||||
|
|||||||
@ -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.
|
||||||
|
"""
|
||||||
|
if not rclpy.ok():
|
||||||
|
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()
|
||||||
|
|
||||||
|
|
||||||
|
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}'"
|
||||||
|
)
|
||||||
@ -0,0 +1,99 @@
|
|||||||
|
"""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."""
|
||||||
|
if not rclpy.ok():
|
||||||
|
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()
|
||||||
|
|
||||||
|
|
||||||
|
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}'"
|
||||||
|
)
|
||||||
@ -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}'"
|
||||||
|
)
|
||||||
@ -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."""
|
||||||
|
if not rclpy.ok():
|
||||||
|
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()
|
||||||
|
|
||||||
|
|
||||||
|
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}'"
|
||||||
|
)
|
||||||
@ -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()
|
||||||
@ -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()
|
||||||
@ -0,0 +1,29 @@
|
|||||||
|
"""Usage example for collaborators.
|
||||||
|
|
||||||
|
Run after sourcing your ROS2 workspace:
|
||||||
|
source install/local_setup.bash
|
||||||
|
python src/fc_network_apps/example_change_mode.py
|
||||||
|
"""
|
||||||
|
|
||||||
|
from fc_network_apps import change_mode
|
||||||
|
|
||||||
|
|
||||||
|
def main() -> None:
|
||||||
|
# Equivalent to:
|
||||||
|
# ros2 service call /fc_network/vehicle/send_command_long ... param1:1 param2:4
|
||||||
|
result = 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}")
|
||||||
|
|
||||||
|
|
||||||
|
if __name__ == "__main__":
|
||||||
|
main()
|
||||||
@ -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()
|
||||||
@ -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()
|
||||||
@ -0,0 +1,64 @@
|
|||||||
|
|
||||||
|
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=3.0,
|
||||||
|
)
|
||||||
|
|
||||||
|
print("=== change mode result ===")
|
||||||
|
print(f"success : {result.success}")
|
||||||
|
print(f"ack_result: {result.ack_result}")
|
||||||
|
print(f"message : {result.message}")
|
||||||
|
|
||||||
|
time.sleep(3)
|
||||||
|
|
||||||
|
result = commandAPI.arm_disarm(
|
||||||
|
target_sysid=3,
|
||||||
|
target_compid=0,
|
||||||
|
arm = True,
|
||||||
|
)
|
||||||
|
|
||||||
|
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.takeoff(
|
||||||
|
target_sysid=3,
|
||||||
|
target_compid=0,
|
||||||
|
altitude_m = 30.0,
|
||||||
|
)
|
||||||
|
|
||||||
|
print("=== change mode result ===")
|
||||||
|
print(f"success : {result.success}")
|
||||||
|
print(f"ack_result: {result.ack_result}")
|
||||||
|
print(f"message : {result.message}")
|
||||||
|
|
||||||
|
# time.sleep(20)
|
||||||
|
|
||||||
|
# result = commandAPI.land(
|
||||||
|
# target_sysid=3,
|
||||||
|
# target_compid=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()
|
||||||
@ -1,111 +0,0 @@
|
|||||||
|
|
||||||
|
|
||||||
import fc_network_apps as fap
|
|
||||||
import time
|
|
||||||
|
|
||||||
def main():
|
|
||||||
commandAPI = fap.CommandLongClient()
|
|
||||||
navAPI = fap.PositionTargetGlobalIntClient()
|
|
||||||
|
|
||||||
target_sysid = 3
|
|
||||||
|
|
||||||
|
|
||||||
print("=== change mode ===")
|
|
||||||
result = fap.CommandLongResult(success=False,message="",ack_result=-1)
|
|
||||||
while not result.success:
|
|
||||||
result.ack_result -= 1
|
|
||||||
result = commandAPI.change_mode(
|
|
||||||
target_sysid=target_sysid,
|
|
||||||
base_mode=1.0,
|
|
||||||
custom_mode=4.0,
|
|
||||||
timeout_sec=3.0,
|
|
||||||
)
|
|
||||||
print(f"success : {result.success}")
|
|
||||||
print(f"ack_result: {result.ack_result}")
|
|
||||||
print(f"message : {result.message}")
|
|
||||||
time.sleep(1)
|
|
||||||
if result.ack_result < -3:
|
|
||||||
return
|
|
||||||
|
|
||||||
print("=== arm vehicle ===")
|
|
||||||
result = fap.CommandLongResult(success=False,message="",ack_result=-1)
|
|
||||||
while not result.success:
|
|
||||||
result.ack_result -= 1
|
|
||||||
result = commandAPI.arm_disarm(
|
|
||||||
target_sysid=target_sysid,
|
|
||||||
arm = True,
|
|
||||||
)
|
|
||||||
print(f"success : {result.success}")
|
|
||||||
print(f"ack_result: {result.ack_result}")
|
|
||||||
print(f"message : {result.message}")
|
|
||||||
time.sleep(1)
|
|
||||||
if result.ack_result < -3:
|
|
||||||
return
|
|
||||||
|
|
||||||
print("=== takeoff ===")
|
|
||||||
result = fap.CommandLongResult(success=False,message="",ack_result=-1)
|
|
||||||
while not result.success:
|
|
||||||
result.ack_result -= 1
|
|
||||||
result = commandAPI.takeoff(
|
|
||||||
target_sysid=target_sysid,
|
|
||||||
altitude_m = 100.0,
|
|
||||||
)
|
|
||||||
print(f"success : {result.success}")
|
|
||||||
print(f"ack_result: {result.ack_result}")
|
|
||||||
print(f"message : {result.message}")
|
|
||||||
time.sleep(1)
|
|
||||||
if result.ack_result < -3:
|
|
||||||
return
|
|
||||||
|
|
||||||
|
|
||||||
time.sleep(15)
|
|
||||||
|
|
||||||
print("=== set_position_target_global_int_deg ===")
|
|
||||||
result_deg = navAPI.goto_position_only(
|
|
||||||
target_sysid=target_sysid,
|
|
||||||
latitude_deg=-35.376655,
|
|
||||||
longitude_deg=149.157011,
|
|
||||||
alt=150.0,
|
|
||||||
timeout_sec=3.0,
|
|
||||||
)
|
|
||||||
print(f"success : {result_deg.success}")
|
|
||||||
print(f"ack_result: {result_deg.ack_result}")
|
|
||||||
print(f"message : {result_deg.message}")
|
|
||||||
print(f"r_lat_int : {result_deg.r_lat_int} r_lon_int: {result_deg.r_lon_int}")
|
|
||||||
|
|
||||||
time.sleep(180)
|
|
||||||
|
|
||||||
result_deg = navAPI.goto_position_only(
|
|
||||||
target_sysid=target_sysid,
|
|
||||||
latitude_deg=-35.3632621,
|
|
||||||
longitude_deg=149.1652375,
|
|
||||||
alt=100.0,
|
|
||||||
timeout_sec=3.0,
|
|
||||||
)
|
|
||||||
print("=== set_position_target_global_int_deg ===")
|
|
||||||
print(f"success : {result_deg.success}")
|
|
||||||
print(f"ack_result: {result_deg.ack_result}")
|
|
||||||
print(f"message : {result_deg.message}")
|
|
||||||
print(f"r_lat_int : {result_deg.r_lat_int} r_lon_int: {result_deg.r_lon_int}")
|
|
||||||
|
|
||||||
time.sleep(180)
|
|
||||||
|
|
||||||
print("=== land ===")
|
|
||||||
result = fap.CommandLongResult(success=False,message="",ack_result=-1)
|
|
||||||
while not result.success:
|
|
||||||
result.ack_result -= 1
|
|
||||||
result = commandAPI.land(
|
|
||||||
target_sysid=3,
|
|
||||||
target_compid=0,
|
|
||||||
)
|
|
||||||
print(f"success : {result.success}")
|
|
||||||
print(f"ack_result: {result.ack_result}")
|
|
||||||
print(f"message : {result.message}")
|
|
||||||
time.sleep(1)
|
|
||||||
if result.ack_result < -3:
|
|
||||||
return
|
|
||||||
|
|
||||||
if __name__ == "__main__":
|
|
||||||
main()
|
|
||||||
|
|
||||||
|
|
||||||
Loading…
Reference in New Issue