跳转至

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
  • 依赖库EarthModelctrl.IpManagergeometry_msgs.msgmathmavros_msgs.msgmavros_msgs.srvnumpyosplatformpymavlinkpymavlink.dialects.v20sensor_msgs.msgsocketstd_msgs.msgstructsubprocesssysthreadingtime
  • 前置准备:调用此接口前,必须先完成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完成通信循环初始化,再依次调用waitForStartMsgsendStartMsg完成握手,顺序错误会导致飞控连接失败,无法接收后续控制指令。
  • 批量操作的连接校验:使用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_QOS
  • 2024-11-22: fix
  • 2024-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: 增加对组播的异常处理,防止断网情况下,初始化报错。