PX4MavCtrlV4ROS 接口文档¶
简介¶
简述:该文件为RflySimSDK中基于MAVLink协议的PX4飞控ROS版控制接口,提供了PX4无人机仿真控制所需的基础封装与数据缓存功能,支持多机仿真场景下按ID寻址的通信管理。
该模块是RflySim无人机仿真平台针对PX4飞控设计的ROS环境通信控制适配模块,遵循平台统一的多机端口分配规则,可以根据无人机ID自动生成对应通信端口,方便用户快速配置多无人机仿真组网场景。它通过fifo类实现有序数据缓存,依托PX4MavCtrler类完成MAVLink协议下的控制指令封装与通信地址管理,适用于RflySim平台中基于ROS开发PX4无人机的自主任务、 swarm集群控制等仿真开发场景,为开发者提供了简洁统一的底层通信控制接口。
快速开始¶
最简可用示例,复制后修改最少配置即可运行。
from RflySimSDK.ctrl import PX4MavCtrler
import time
if __name__ == "__main__":
# 初始化PX4无人机控制器,使用默认本机UDP连接,控制ID为1号无人机
px4_ctrl = PX4MavCtrler(CopterID=1, ip="127.0.0.1", Com="udp", port=0)
# 启动控制器消息循环,接收并处理飞控发来的数据
px4_ctrl.spin()
# 保持运行5秒,验证连接正常
time.sleep(5)
print("PX4控制器初始化和消息循环运行成功")
环境与依赖¶
- 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):无
进阶用法示例¶
展示复杂组合场景(如多类协作、异步控制、批量操作)
该示例实现了多PX4无人机的批量初始化、异步武装与异步位置控制,利用fifo队列缓存多机控制指令,实现多机协作场景下的异步任务调度:
import RflySimSDK.ctrl as rfly_ctrl
from threading import Thread
# 初始化10架无人机的控制器列表与fifo指令缓存队列
drone_list = []
cmd_fifo = rfly_ctrl.fifo()
for i in range(10):
ctrl = rfly_ctrl.PX4MavCtrler(id=i+1)
ctrl.InitMavLoop()
ctrl.fillList(drone_list)
drone_list.append(ctrl)
# 异步处理线程:从fifo读取指令并批量下发
def async_cmd_publish():
while True:
cmd = cmd_fifo.read()
if cmd is None:
break
for ctrl in drone_list[cmd['drone_range']]:
ctrl.setGPSOriLLA(cmd['lat'], cmd['lon'], cmd['alt'])
# 启动异步处理线程
pub_thread = Thread(target=async_cmd_publish, daemon=True)
pub_thread.start()
# 批量完成起飞前准备与武装
start_success = True
for ctrl in drone_list:
if not ctrl.waitForStartMsg():
start_success = False
break
ctrl.sendStartMsg()
ctrl.SendMavArm()
if start_success:
# 批量写入起飞位置指令到fifo
cmd_fifo.write({'drone_range': slice(0,10), 'lat': 30.5, 'lon': 104.0, 'alt': 100})
# 启动消息循环处理飞控回执
for ctrl in drone_list:
Thread(target=ctrl.spin, daemon=True).start()
注意事项与避坑指南¶
- 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: 增加对组播的异常处理,防止断网情况下,初始化报错。