PX4MavCtrlV4ROS 接口文档¶
简介¶
简述：该文件为RflySimSDK中基于MAVLink协议的PX4飞控ROS版控制接口，提供了PX4无人机仿真控制所需的基础封装与数据缓存功能，支持多机仿真场景下按ID寻址的通信管理。
该模块是RflySim无人机仿真平台针对PX4飞控设计的ROS环境通信控制适配模块，遵循平台统一的多机端口分配规则，可以根据无人机ID自动生成对应通信端口，方便用户快速配置多无人机仿真组网场景。它通过fifo类实现有序数据缓存，依托PX4MavCtrler类完成MAVLink协议下的控制指令封装与通信地址管理，适用于RflySim平台中基于ROS开发PX4无人机的自主任务、 swarm集群控制等仿真开发场景，为开发者提供了简洁统一的底层通信控制接口。
快速开始¶
以下示例在 ROS 环境中控制 1 号无人机进入 Offboard，发送位置指令后切换为 FRD 机体系速度控制。
参考例程：[RflySim安装路径]\RflySimAPIs\6.RflySimExtCtrl\0.ApiExps\e18_MavrosExps\1.RflyRosAPIOneUav
import time
import PX4MavCtrlV4ROS as PX4MavCtrl
mav = PX4MavCtrl.PX4MavCtrler(CopterID=1)
time.sleep(1)
mav.initOffboard()
time.sleep(5)
# 飞至 NED 坐标 (0.1, 0, -0.8)
mav.SendPosNED(0.1, 0, -0.8)
time.sleep(10)
print("PosE", mav.uavPosNED)
print("VelE", mav.uavVelNED)
# 以 0.1 m/s 向机头前方飞行，同时缓慢偏航
mav.SendVelFRD(0.1, 0, 0, 0.05)
环境与依赖¶
- Python 环境：
>= 3.8.10 - 依赖库：
EarthModel、ctrl.IpManager、geometry_msgs.msg、math、mavros_msgs.msg、mavros_msgs.srv、numpy、os、platform、pymavlink、pymavlink.dialects.v20、sensor_msgs.msg、socket、std_msgs.msg、struct、subprocess、sys、threading、time - 前置准备：调用此接口前，必须先完成RflySimSDK的环境配置并确保PX4飞控仿真环境已就绪。
核心接口说明¶
该模块 PX4MavCtrlV4ROS.py 包含了配置变量、辅助函数及核心业务类。
全局常量与枚举定义¶
本节列出模块中所有可直接引用的全局常量和枚举定义。
独立常量¶
无
全局/独立函数¶
无
fifo 类¶
先进先出（FIFO）队列工具类，用于存储和读取数据，遵循先进先出的数据访问规则，一般用于数据缓存场景。
__init__()¶
功能说明：初始化空的先进先出队列实例 参数列表 (Args)： 无 返回值 (Returns)：
fifo实例对象
异常 (Raises)： 无
write(data)¶
功能说明：向先进先出队列末尾写入数据 参数列表 (Args)：
| 参数名 | 类型 | 是否必填 | 默认值 | 说明 |
|---|---|---|---|---|
data |
Any |
是 | - | 需要写入队列的数据 |
返回值 (Returns)： 无 异常 (Raises)： 无
示例：
from RflySimSDK.ctrl.PX4MavCtrlV4ROS import fifo
fifo_buffer = fifo()
fifo_buffer.write(10)
fifo_buffer.write("hello")
read()¶
功能说明：从先进先出队列头部读取并移除最早写入的数据 参数列表 (Args)： 无 返回值 (Returns)：
Any: 队列中最早写入的数据，若队列为空返回 None
异常 (Raises)： 无
示例：
from RflySimSDK.ctrl.PX4MavCtrlV4ROS import fifo
fifo_buffer = fifo()
fifo_buffer.write(10)
fifo_buffer.write("hello")
first_data = fifo_buffer.read()
# first_data 为 10
PX4MavCtrler 类¶
PX4飞控MAVLink控制器，支持UDP仿真通信和串口直连飞控两种连接模式，用于实现对PX4无人机的控制与状态获取。
__init__(CopterID=1, ip='127.0.0.1', Com='udp', port=0)¶
功能说明：初始化PX4飞控MAVLink控制器，根据参数配置建立与飞控的通信连接。 参数列表 (Args)：
| 参数名 | 类型 | 默认值 | 说明 |
|---|---|---|---|
| CopterID | int | 1 | 无人机的ID编号，默认按照平台规则自动计算通信端口，port=20100+CopterID*2-2 |
| ip | str | 127.0.0.1 | MAVLink数据发送的目标IP地址。默认发往本机地址，分布式仿真可指定局域网IP，也可使用广播地址255.255.255.255 |
| Com | str | udp | 与Pixhawk飞控的连接模式：'udp'表示使用UDP模式接收CopterSim转发的MAVLink消息；也可填写Windows下的'COM3'或Linux下的'/dev/ttyUSB0'等串口名称，启用串口直连模式 |
| port | int | 0 | UDP模式下若设为0则自动根据CopterID计算端口，赋值大于0时强制使用指定端口；串口模式下该参数表示波特率，为0时默认使用57600波特率 |
返回值 (Returns)：无 异常 (Raises)：无
spin()¶
功能说明：ROS节点自旋处理，维持节点运行处理回调函数 参数列表 (Args)： 无参数 返回值 (Returns)：无返回值 异常 (Raises)：无
convert_to_payload64(payload_bytes)¶
功能说明：将输入字节数据转换为符合MAVLink要求的64字节载荷格式 参数列表 (Args)：
| 参数名 | 类型 | 是否必填 | 默认值 | 说明 |
|---|---|---|---|---|
| payload_bytes | bytes | 是 | 无 | 需要转换的原始字节数据 |
返回值 (Returns)：转换完成的64字节格式载荷 异常 (Raises)：无
convert_to_rosmsg(mavmsg, header)¶
功能说明：将pymavlink格式消息转换为ROS标准Mavlink消息格式，目前仅支持MAVLink v1.0 参数列表 (Args)：
| 参数名 | 类型 | 是否必填 | 默认值 | 说明 |
|---|---|---|---|---|
| mavmsg | pymavlink.message | 是 | 无 | 输入的pymavlink格式消息 |
| header | std_msgs/Header | 是 | 无 | ROS消息头，用于填充输出ROS消息的头信息 |
返回值 (Returns)：转换完成的ROS标准Mavlink消息 异常 (Raises)：无
setGPSOriLLA(LonLatAlt)¶
功能说明：设置无人机初始GPS经纬度与高度信息 参数列表 (Args)：
| 参数名 | 类型 | 是否必填 | 默认值 | 说明 |
|---|---|---|---|---|
| LonLatAlt | list[float] | 否 | [40.1540302, 116.2593683, 50] |
初始GPS信息，格式为[经度, 纬度, 高度] |
返回值 (Returns)：无返回值 异常 (Raises)：无
arm_px4(isArm)¶
功能说明：控制PX4无人机解锁或上锁 参数列表 (Args)：
| 参数名 | 类型 | 是否必填 | 默认值 | 说明 |
|---|---|---|---|---|
| isArm | bool | 是 | 无 | True表示解锁无人机，False表示上锁无人机 |
返回值 (Returns)：无返回值 异常 (Raises)：无
fillList(data, inLen, fill)¶
功能说明：将输入列表填充/截断到指定长度 参数列表 (Args)：
| 参数名 | 类型 | 是否必填 | 默认值 | 说明 |
|---|---|---|---|---|
| data | list | 是 | 无 | 输入的原始数据列表 |
| inLen | int | 是 | 无 | 目标输出列表长度 |
| fill | any | 否 | 0 |
当原始列表长度不足时，用于填充的元素 |
返回值 (Returns)：填充/截断后长度为inLen的新列表
异常 (Raises)：无
InitMavLoop()¶
功能说明：初始化MAVLink通信循环，启动通信相关线程与处理逻辑 参数列表 (Args)： 无参数 返回值 (Returns)：无返回值 异常 (Raises)：无
sendStartMsg(copterID)¶
功能说明：发送启动消息到指定ID无人机，完成初始化握手 参数列表 (Args)：
| 参数名 | 类型 | 是否必填 | 默认值 | 说明 |
|---|---|---|---|---|
| copterID | int | 否 | -1 |
目标无人机ID，-1表示广播给所有无人机 |
返回值 (Returns)：无返回值 异常 (Raises)：无
waitForStartMsg()¶
功能说明：等待无人机返回启动确认消息，完成初始化握手流程 参数列表 (Args)： 无参数 返回值 (Returns)：无返回值 异常 (Raises)：无
SendMavArm(isArm)¶
功能说明：通过MAVLink发送无人机上锁/解锁命令 参数列表 (Args)：
| 参数名 | 类型 | 是否必填 | 默认值 | 说明 |
|---|---|---|---|---|
| isArm | bool | 是 | 无 | True发送解锁命令，False发送上锁命令 |
返回值 (Returns)：无返回值 异常 (Raises)：无
initOffboard()¶
功能说明：初始化PX4离线控制(Offboard)模式，进入离线控制准备状态 参数列表 (Args)： 无参数 返回值 (Returns)：无返回值 异常 (Raises)：无
OffboardLoop()¶
功能说明：离线控制循环，持续发送离线控制指令给PX4飞控 参数列表 (Args)： 无参数 返回值 (Returns)：无返回值 异常 (Raises)：无
endOffboard()¶
功能说明：退出PX4离线控制(Offboard)模式，停止发送离线控制指令 参数列表 (Args)： 无参数 返回值 (Returns)：无返回值 异常 (Raises)：无
stopRun()¶
功能说明：停止PX4MavCtrler所有运行中的线程与循环，安全关闭通信 参数列表 (Args)： 无参数 返回值 (Returns)：无返回值 异常 (Raises)：无
calcTypeMask(EnList)¶
功能说明：根据使能字段列表计算MAVLink离线控制指令的类型掩码 参数列表 (Args)：
| 参数名 | 类型 | 是否必填 | 默认值 | 说明 |
|---|---|---|---|---|
| EnList | list[bool] | 是 | 无 | 各控制字段使能列表，对应位置True表示使能该字段 |
返回值 (Returns)：计算得到的类型掩码整数 异常 (Raises)：无
SendVelNED(vx=math.nan, vy=math.nan, vz=math.nan, yawrate=math.nan)¶
功能说明：向北东下(NED)坐标系发送速度控制指令，若参数为nan则对应通道保持原控制量不变
参数列表 (Args)：
| 参数名 | 类型 | 是否必填 | 默认值 | 说明 |
|---|---|---|---|---|
| vx | float | 否 | math.nan | 北向速度，单位 m/s |
| vy | float | 否 | math.nan | 东向速度，单位 m/s |
| vz | float | 否 | math.nan | 下向速度，单位 m/s |
| yawrate | float | 否 | math.nan | 偏航角速度，单位 rad/s |
| 返回值 (Returns)：无返回值 |
异常 (Raises)：无
SendVelFRD(vx=math.nan, vy=math.nan, vz=math.nan, yawrate=math.nan)¶
功能说明：向前右下(FRD)机体坐标系发送速度控制指令，若参数为nan则对应通道保持原控制量不变
参数列表 (Args)：
| 参数名 | 类型 | 是否必填 | 默认值 | 说明 |
|---|---|---|---|---|
| vx | float | 否 | math.nan | 前向速度，单位 m/s |
| vy | float | 否 | math.nan | 右向速度，单位 m/s |
| vz | float | 否 | math.nan | 下向速度，单位 m/s |
| yawrate | float | 否 | math.nan | 偏航角速度，单位 rad/s |
| 返回值 (Returns)：无返回值 |
异常 (Raises)：无
SendPosNED(x=math.nan, y=math.nan, z=math.nan, yaw=math.nan)¶
功能说明：向北东下(NED)坐标系发送位置控制指令，若参数为nan则对应通道保持原控制量不变
参数列表 (Args)：
| 参数名 | 类型 | 是否必填 | 默认值 | 说明 |
|---|---|---|---|---|
| x | float | 否 | math.nan | 北向位置，单位 m |
| y | float | 否 | math.nan | 东向位置，单位 m |
| z | float | 否 | math.nan | 下向位置，单位 m |
| yaw | float | 否 | math.nan | 偏航角，单位 rad |
| 返回值 (Returns)：无返回值 |
异常 (Raises)：无
SendPosVelNED(PosE=[math.nan] * 3, VelE=[math.nan] * 3, yaw=math.nan, yawrate=math.nan)¶
功能说明：向北东下(NED)坐标系同时发送位置和速度控制指令，若参数为nan则对应通道保持原控制量不变
参数列表 (Args)：
| 参数名 | 类型 | 是否必填 | 默认值 | 说明 |
|---|---|---|---|---|
| PosE | list[float] | 否 | [math.nan] * 3 | 北东下位置，依次为[x, y, z]，单位 m |
| VelE | list[float] | 否 | [math.nan] * 3 | 北东下速度，依次为[vx, vy, vz]，单位 m/s |
| yaw | float | 否 | math.nan | 偏航角，单位 rad |
| yawrate | float | 否 | math.nan | 偏航角速度，单位 rad/s |
| 返回值 (Returns)：无返回值 |
异常 (Raises)：无
local_pose_callback(msg)¶
功能说明：ROS本地位置话题回调函数，用于更新存储的无人机本地位姿信息 参数列表 (Args)：
| 参数名 | 类型 | 是否必填 | 默认值 | 说明 |
|---|---|---|---|---|
| msg | geometry_msgs/PoseStamped | 是 | 无 | ROS本地位置话题消息 |
| 返回值 (Returns)：无返回值 |
异常 (Raises)：无
local_vel_callback(msg)¶
功能说明：ROS本地速度话题回调函数，用于更新存储的无人机本地速度信息 参数列表 (Args)：
| 参数名 | 类型 | 是否必填 | 默认值 | 说明 |
|---|---|---|---|---|
| msg | geometry_msgs/TwistStamped | 是 | 无 | ROS本地速度话题消息 |
| 返回值 (Returns)：无返回值 |
异常 (Raises)：无
mavros_state_callback(msg)¶
功能说明：mavros状态话题回调函数，用于更新PX4的连接状态、解锁状态和飞行模式信息 参数列表 (Args)：
| 参数名 | 类型 | 是否必填 | 默认值 | 说明 |
|---|---|---|---|---|
| msg | mavros_msgs/State | 是 | 无 | mavros状态话题消息 |
| 返回值 (Returns)：无返回值 |
异常 (Raises)：无
imu_callback(msg)¶
功能说明：IMU数据话题回调函数，用于更新存储的IMU加速度与角速度信息 参数列表 (Args)：
| 参数名 | 类型 | 是否必填 | 默认值 | 说明 |
|---|---|---|---|---|
| msg | sensor_msgs/Imu | 是 | 无 | ROS IMU话题消息 |
| 返回值 (Returns)：无返回值 |
异常 (Raises)：无
gps_callback(msg)¶
功能说明：GPS数据话题回调函数，用于更新存储的GPS信息 参数列表 (Args)：
| 参数名 | 类型 | 是否必填 | 默认值 | 说明 |
|---|---|---|---|---|
| msg | sensor_msgs/NavSatFix | 是 | 无 | ROS GPS话题消息 |
| 返回值 (Returns)：无返回值 |
异常 (Raises)：无
q2yaw(q)¶
功能说明：将四元数转换为偏航角yaw 参数列表 (Args)：
| 参数名 | 类型 | 是否必填 | 默认值 | 说明 |
|---|---|---|---|---|
| q | list[float]/geometry_msgs/Quaternion | 是 | 无 | 输入四元数，格式为[w, x, y, z]或四元数消息 |
| 返回值 (Returns)：float 计算得到的偏航角，单位 rad |
异常 (Raises)：无
q2Euler(q)¶
功能说明：将四元数转换为滚动角roll、俯仰角pitch、偏航角yaw 参数列表 (Args)：
| 参数名 | 类型 | 是否必填 | 默认值 | 说明 |
|---|---|---|---|---|
| q | list[float]/geometry_msgs/Quaternion | 是 | 无 | 输入四元数，格式为[w, x, y, z]或四元数消息 |
| 返回值 (Returns)：tuple[float, float, float] 依次为(roll, pitch, yaw)，单位 rad |
异常 (Raises)：无
SendMavArm(isArm=1)¶
功能说明：发送无人机解锁/上锁指令 参数列表 (Args)：
| 参数名 | 类型 | 是否必填 | 默认值 | 说明 |
|---|---|---|---|---|
| isArm | int | 否 | 1 | 1表示发送解锁指令，0表示发送上锁指令 |
| 返回值 (Returns)：无返回值 |
异常 (Raises)：无
arm()¶
功能说明：执行无人机解锁操作 参数列表 (Args)：无参数 返回值 (Returns)：无返回值 异常 (Raises)：无
disarm()¶
功能说明：执行无人机上锁操作 参数列表 (Args)：无参数 返回值 (Returns)：无返回值 异常 (Raises)：无
offboard()¶
功能说明：切换PX4到Offboard飞行模式，允许外部发送控制指令 参数列表 (Args)：无参数 返回值 (Returns)：无返回值 异常 (Raises)：无
land()¶
功能说明：向PX4发送自动降落指令，使无人机进入降落模式执行自动降落 参数列表 (Args)： 无参数 返回值 (Returns)：无返回值 异常 (Raises)：无
yawSat(yaw)¶
功能说明：对无人机的偏航角指令进行限幅处理，限制偏航角速度的最大变化范围 参数列表 (Args)：
| 参数名 | 类型 | 是否必填 | 默认值 | 说明 |
|---|---|---|---|---|
| yaw | float |
是 | 无 | 待限幅的目标偏航角指令，单位为弧度 |
返回值 (Returns)：float 经过限幅处理后的目标偏航角指令
异常 (Raises)：无
sendMavSetParam(param_id_str, param_value, param_type)¶
功能说明：通过MAVLink向PX4飞控发送参数设置请求，修改飞控指定参数的值 参数列表 (Args)：
| 参数名 | 类型 | 是否必填 | 默认值 | 说明 |
|---|---|---|---|---|
| param_id_str | str |
是 | 无 | 待设置的参数名称字符串，对应PX4飞控的参数ID |
| param_value | float/int |
是 | 无 | 待设置的目标参数值 |
| param_type | int |
是 | 无 | MAVLink参数类型标识，对应MAV_PARAM_TYPE枚举值 |
返回值 (Returns)：无返回值 异常 (Raises)：无
SendMavCmdLong(command, param1=0, param2=0, param3=0, param4=0, param5=0, param6=0, param7=0)¶
功能说明：发送MAVLink长命令（COMMAND_LONG消息）到PX4飞控，用于执行各类飞控指令 参数列表 (Args)：
| 参数名 | 类型 | 是否必填 | 默认值 | 说明 |
|---|---|---|---|---|
| command | int |
是 | 无 | MAVLink命令ID，对应MAV_CMD枚举定义的指令 |
| param1 | float |
否 | 0 | MAVLink命令参数1，具体含义由指令ID决定 |
| param2 | float |
否 | 0 | MAVLink命令参数2，具体含义由指令ID决定 |
| param3 | float |
否 | 0 | MAVLink命令参数3，具体含义由指令ID决定 |
| param4 | float |
否 | 0 | MAVLink命令参数4，具体含义由指令ID决定 |
| param5 | float |
否 | 0 | MAVLink命令参数5，具体含义由指令ID决定 |
| param6 | float |
否 | 0 | MAVLink命令参数6，具体含义由指令ID决定 |
| param7 | float |
否 | 0 | MAVLink命令参数7，具体含义由指令ID决定 |
返回值 (Returns)：无返回值 异常 (Raises)：无
SendHILCtrlMsg(ctrls=[0] * 16, idx=0)¶
功能说明：向PX4发送HIL_ACTUATOR_CONTROLS控制消息，该消息会被PX4转换为uORB的rfly_ctrl消息，用于硬件在环仿真中发送舵机控制指令。对应MAVLink消息定义：https://mavlink.io/en/messages/common.html#HIL_ACTUATOR_CONTROLS
参数列表 (Args)：
| 参数名 | 类型 | 是否必填 | 默认值 | 说明 |
|---|---|---|---|---|
| ctrls | list[float] |
否 | [0] * 16 |
16路舵机/控制量输出值，范围通常对应飞控的控制输出归一化范围 |
| idx | int |
否 | 0 | 控制消息组索引，标识当前控制量所属的分组 |
返回值 (Returns)：无返回值 异常 (Raises)：无
进阶用法示例¶
以下示例为 1～3 号无人机分别创建 ROS 控制器，批量执行起飞和 FRD 速度控制。
参考例程：[RflySim安装路径]\RflySimAPIs\6.RflySimExtCtrl\0.ApiExps\e21.RosTransExps\1.C++Demo\Simulink--dll
import time
import PX4MavCtrlV4ROS as PX4MavCtrl
mavList = [
PX4MavCtrl.PX4MavCtrler(CopterID=copterID)
for copterID in [1, 2, 3]
]
time.sleep(1)
# 依次进入 Offboard 并发送起飞位置
for mav in mavList:
mav.initOffboard()
mav.SendPosNED(0.1, 0, -0.8)
time.sleep(1)
time.sleep(10)
# 三架无人机同时执行机体系速度控制
for mav in mavList:
mav.SendVelFRD(0.1, 0, 0, 0.05)
while True:
print("VelE", *[mav.uavVelNED for mav in mavList])
time.sleep(3)
注意事项与避坑指南¶
- fifo读写线程安全问题：fifo类本身未内置多线程锁，多线程同时读写fifo时可能出现指令重复读取、数据错乱问题，需要用户自行加锁保证操作安全。
- 初始化消息交互顺序要求：必须先调用
InitMavLoop完成通信循环初始化，再依次调用waitForStartMsg、sendStartMsg完成握手，顺序错误会导致飞控连接失败，无法接收后续控制指令。 - 批量操作的连接校验：使用
fillList批量添加控制器后，需要逐个校验waitForStartMsg的返回值，单架无人机连接失败会影响整个集群任务，不能跳过校验直接执行后续操作。 - 武装接口的使用区别：
arm_px4为直接封装的武装指令接口，SendMavArm为适配PX4消息协议的标准武装接口，v4版本ROS通信链路必须使用SendMavArm，否则会出现指令无法解析的问题。
更新日志¶
2026-03-03: feat：SDK增加IP处理机制，兼容本地版上云2025-09-11: fix: 增加Linux端口复用的支持2025-09-01: 增加在spin 挂起函数，在外部只要使用这个接口spin函数就能将守护进程挂起2025-08-27: fix: 更新速度订阅接口2025-03-31: fix: 修复1.13之后版本，Offboard无法起飞的问题。2025-01-17: fix：修复ROS1环境下调用QOS配置错误2025-01-07: fix: sensor related date should use SENSOR_QOS2024-11-22: fix2024-06-13: fix：更新PX4MavCtrlV4ROS.py文件中注释2024-06-05: fix：更新ROS接口2024-06-04: fix: 修复ROS2兼容性2024-06-04: fix: 适配更多的2024-06-04: fix: 更新库文件2024-03-11: Fix:接口无效，需要屏蔽2024-03-03: fix: 增加对组播的异常处理，防止断网情况下，初始化报错。