Compare commits
4 Commits
f34693a400
...
b0f1bd56f5
| Author | SHA1 | Date |
|---|---|---|
 Chiyu Chen
|
b0f1bd56f5 | 4 weeks ago |
|
Chiyu Chen
|
2990de2f3f | 4 weeks ago |
|
Chiyu Chen
|
cf213fc556 | 4 weeks ago |
|
Chiyu Chen
|
017dd4923d | 4 weeks ago |
@ -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",
|
||||
]
|
||||
|
||||
@ -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}'"
|
||||
)
|
||||
@ -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}'"
|
||||
)
|
||||
@ -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}'"
|
||||
)
|
||||
@ -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}'"
|
||||
)
|
||||
@ -1,55 +0,0 @@
|
||||
|
||||
|
||||
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()
|
||||
@ -1,30 +0,0 @@
|
||||
"""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()
|
||||
@ -1,29 +0,0 @@
|
||||
"""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()
|
||||
@ -1,27 +0,0 @@
|
||||
"""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()
|
||||
@ -1,28 +0,0 @@
|
||||
"""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()
|
||||
@ -1,64 +0,0 @@
|
||||
|
||||
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()
|
||||
@ -0,0 +1,111 @@
|
||||
|
||||
|
||||
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