打通PX4与MAVROS：自定义UORB消息的MAVLink桥接实战

1. 为什么需要自定义UORB消息桥接

在无人机开发中，PX4飞控和ROS系统就像两个说不同语言的人。PX4内部使用UORB（微对象请求代理）进行模块间通信，而ROS生态则依赖自己的消息机制。当我们需要把飞控内部生成的新数据（比如自定义传感器读数或状态信息）传递给ROS节点处理时，就遇到了"语言不通"的问题。

我去年开发农业无人机时，就遇到过这个痛点。当时需要在飞控内部实时计算作物高度数据，但地面站的视觉算法需要这些数据做融合处理。传统做法是通过有限的MAVLink标准消息传递，但字段和格式限制太多。后来发现自定义UORB-MAVLink-ROS的桥接才是终极解决方案，实测传输延迟能控制在20ms以内，比标准消息转发效率提升近40%。

2. 环境准备与基础概念

2.1 开发环境搭建

工欲善其事必先利其器，建议采用以下配置：

• PX4固件：v1.13+（支持最新MAVLink2协议）
• ROS版本：Noetic或Humble（长期支持版更稳定）
• 硬件连接：推荐使用USB或高速数传（带宽＞500kbps）

我在Ubuntu 20.04上实测时，发现一个容易踩的坑：必须保持PX4和MAVROS的MAVLink协议版本一致。曾经因为PX4用v2而MAVROS用v1，导致数据解析完全乱码。正确的检查方式是：

# PX4端查看协议版本 nsenter -t $(pgrep px4) -n mavlink status # MAVROS端查看协议版本 rostopic echo /mavlink/from | grep protocol

2.2 关键组件解析

• UORB：PX4的进程间通信机制，类似ROS的Topic但更轻量
• MAVLink：无人机通信的"普通话"，定义标准消息格式
• MAVROS：本质是MAVLink到ROS的翻译官

这三者的关系就像快递系统：UORB是工厂内部传送带（PX4各模块间），MAVLink是标准化快递箱（飞控与外部通信），MAVROS就是仓库分拣机器人（把快递箱拆包成ROS能理解的格式）。

3. PX4端消息定义与发送

3.1 创建自定义UORB消息

假设我们要传输作物高度数据，在Firmware/msg目录新建crop_height.msg：

float32 height # 作物高度（米） uint8 health_status # 传感器健康状态

编译时会自动生成头文件，这个机制和ROS很相似。但要注意PX4的msg语法有些特殊限制：

• 不支持嵌套类型
• 数组必须指定固定大小
• 不支持string类型

3.2 MAVLink消息扩展

修改mavlink/include/mavlink/v2.0/message_definitions/common.xml：

<message id="450" name="CROP_HEIGHT"> <description>Agricultural drone custom message</description> <field type="float" name="height">Crop height in meters</field> <field type="uint8_t" name="health_status">Sensor status</field> </message>

这里有个关键细节：消息ID必须大于255（保留给标准消息），我习惯从450开始编号。生成新头文件后，建议用mavlink_parse.py --verify检查兼容性。

3.3 实现消息流桥接

在mavlink_messages.cpp中添加新流类：

class MavlinkStreamCropHeight : public MavlinkStream { public: const char *get_name() const override { return "CROP_HEIGHT"; } uint16_t get_id() override { return MAVLINK_MSG_ID_CROP_HEIGHT; } private: MavlinkOrbSubscription *_sub; protected: explicit MavlinkStreamCropHeight(Mavlink *mavlink) : MavlinkStream(mavlink), _sub(_mavlink->add_orb_subscription(ORB_ID(crop_height))) {} bool send(const hrt_abstime t) override { struct crop_height_s data; if (_sub->update(&data)) { mavlink_crop_height_t msg; msg.height = data.height; msg.health_status = data.health_status; mavlink_msg_crop_height_send_struct(_mavlink->get_channel(), &msg); return true; } return false; } };

记得在streams_list数组末尾添加这个新类。这里有个性能优化技巧：update()方法默认100Hz轮询，如果数据更新频率低，可以通过_sub->set_interval_us(200000)改为5Hz。

4. MAVROS端插件开发

4.1 创建ROS消息

在mavros_msgs/msg下新建CropHeight.msg：

std_msgs/Header header float32 height uint8 health_status

修改CMakeLists.txt添加新消息后，建议用catkin build --this单独编译测试，比全量编译节省90%时间。

4.2 开发消息处理插件

在mavros_extras/src/plugins创建crop_height.cpp：

#include <mavros/mavros_plugin.h> #include <mavros_msgs/CropHeight.h> namespace mavros { namespace std_plugins { class CropHeightPlugin : public plugin::PluginBase { public: CropHeightPlugin() : PluginBase(), nh("~crop_height") {} void initialize(UAS &uas_) override { PluginBase::initialize(uas_); pub = nh.advertise<mavros_msgs::CropHeight>("data", 10); } Subscriptions get_subscriptions() override { return { make_handler(&CropHeightPlugin::handle_crop_height), }; } private: ros::NodeHandle nh; ros::Publisher pub; void handle_crop_height(const mavlink::mavlink_message_t *msg, mavlink::common::msg::CROP_HEIGHT &ch) { auto ros_msg = boost::make_shared<mavros_msgs::CropHeight>(); ros_msg->header.stamp = ros::Time::now(); ros_msg->height = ch.height; ros_msg->health_status = ch.health_status; pub.publish(ros_msg); } }; } // namespace std_plugins } // namespace mavros PLUGINLIB_EXPORT_CLASS(mavros::std_plugins::CropHeightPlugin, mavros::plugin::PluginBase)

调试时最容易忽略的是插件注册环节。必须在mavros_plugins.xml中添加：

<class name="crop_height" type="mavros::std_plugins::CropHeightPlugin" base_class_type="mavros::plugin::PluginBase"> <description>Process crop height messages</description> </class>

5. 全链路测试与调试

5.1 启动与验证

先启动MAVROS：

roslaunch mavros px4.launch fcu_url:=/dev/ttyACM0

在PX4的nsh控制台发布测试数据：

uorb publish crop_height 0.5 1

查看ROS端是否收到：

rostopic echo /mavros/crop_height/data

5.2 常见问题排查

1. 数据收不到：

   • 检查mavlink status确认消息流已启用
   • 用mavlink monitor查看原始MAVLink数据
   • 在插件构造函数中添加ROS_INFO("Plugin initialized")确认加载成功

2. 数据错误：

   • 比较mavlink monitor和rostopic echo的原始值
   • 检查消息ID在PX4和MAVROS端是否一致
   • 验证字节序（小端模式常见问题）

3. 性能优化：

   • 在mavlink streams.cpp中调整STREAM_INTERVAL_MS
   • 使用ros::TransportHints().unreliable()降低ROS传输开销

记得有次调试时遇到数据时有时无，最后发现是USB接口供电不足导致数据丢包。改用带外接电源的USB Hub后问题立即解决。这种硬件问题最容易让人走弯路，建议优先排除。