(tested) 1. 增加各種 command long 的功能與使用範例 2. 刪除無用的 mavlinkPublish.py 3.
parent
e4585134cc
commit
44d53f51fb
@ -1,212 +0,0 @@
|
||||
|
||||
'''
|
||||
這個檔案只有一個 class
|
||||
是作為 mavlinkObject.py 中 mavlink_analyzer 類別的功能衍生
|
||||
|
||||
主要概念是將 "離散的" mavlink 參數轉換成 ROS topic
|
||||
包含了創建 publisher 和 以及包裝並丟到 ros2 topic 的 packEmit
|
||||
|
||||
publisher topic name 命名規則為 <前綴詞>/s<sysid>/<具體 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 訊息 請放在這裡 ↑↑↑↑↑↑↑↑↑↑↑↑
|
||||
@ -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",
|
||||
]
|
||||
|
||||
@ -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}'"
|
||||
)
|
||||
@ -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,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,
|
||||
)
|
||||
@ -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}'"
|
||||
)
|
||||
@ -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,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()
|
||||
Loading…
Reference in New Issue