ROS2 在 PX4 飞控中的应用与实践-优快云博客

ROS2（Robot Operating System 2）是一套用于构建机器人应用的灵活框架，它提供了分布式通信、模块化设计和跨平台支持等核心特性。相比 ROS1，ROS2 在实时性、安全性和可靠性方面有了显著提升，使其更适合用于无人机等对实时性要求较高的领域。

1.2 PX4 飞控系统简介

PX4 是一个开源的无人机飞控系统，支持多种飞行器类型（多旋翼、固定翼、垂直起降等）。它包含传感器融合、姿态控制、位置控制等核心功能，通过 MAVLink 协议与地面站和其他系统通信。

1.3 ROS2 与 PX4 结合的优势

分布式架构：便于构建复杂的无人机系统
丰富的生态系统：可利用 ROS2 社区的大量包和工具
实时性支持：满足飞控系统的时间敏感需求
模块化设计：便于功能扩展和维护
跨平台兼容性：支持多种硬件和操作系统

2. ROS2 与 PX4 的集成架构

2.1 整体架构

ROS2 与 PX4 的集成主要通过以下组件实现：

PX4 固件：运行在飞控硬件上的核心控制软件
MAVLink 协议：无人机系统间的通信协议
ROS2 MAVLink 桥接器：如 mavros 或 px4_ros_com，实现 ROS2 与 MAVLink 的转换
ROS2 节点：实现各种应用功能的模块

plaintext

┌─────────────┐      ┌──────────────┐      ┌─────────────┐
│  ROS2应用   │<────>│ MAVLink桥接器 │<────>│  PX4固件    │
│  (节点群)   │      │ (mavros等)   │      │ (飞控硬件)  │
└─────────────┘      └──────────────┘      └─────────────┘
        ↑                    ↑                    ↑
        │                    │                    │
┌─────────────┐      ┌──────────────┐      ┌─────────────┐
│传感器处理节点│      │  通信协议转换 │      │  飞行控制   │
│规划与决策节点│      │  (ROS2-MAVLink)│    │  姿态控制   │
│执行与反馈节点│      │              │      │  位置控制   │
└─────────────┘      └──────────────┘      └─────────────┘

2.2 通信流程

ROS2 节点通过 ROS2 话题 / 服务发布控制指令
桥接器将 ROS2 消息转换为 MAVLink 消息
PX4 接收 MAVLink 消息并执行相应控制
PX4 将状态信息通过 MAVLink 反馈
桥接器将 MAVLink 消息转换为 ROS2 消息
ROS2 节点接收并处理反馈信息

3. 核心应用场景与案例

3.1 自主导航与路径规划

ROS2 提供了丰富的路径规划算法和导航栈（如 Nav2），可与 PX4 结合实现无人机的自主导航功能。

3.2 目标检测与跟踪

结合 ROS2 的计算机视觉库（如 OpenCV、Vision_msgs），实现无人机对特定目标的识别与跟踪。

3.3 多机协同控制

利用 ROS2 的分布式通信能力，实现多架无人机的协同作业。

3.4 传感器数据融合

通过 ROS2 整合多种传感器数据（如激光雷达、视觉传感器），提升无人机的环境感知能力。

3.5 任务规划与执行

实现复杂任务的规划与执行，如自主巡检、快递配送等。

4. 代码实现与详细注释

4.1 环境配置

首先需要安装 ROS2 和 PX4 的相关依赖：

bash

# 安装ROS2 Foxy (示例，根据实际版本调整)
sudo apt install ros-foxy-desktop

# 安装MAVROS
sudo apt install ros-foxy-mavros ros-foxy-mavros-extras

# 安装 geographiclib 数据集
wget https://raw.githubusercontent.com/mavlink/mavros/master/mavros/scripts/install_geographiclib_datasets.sh
sudo bash ./install_geographiclib_datasets.sh

# 安装其他依赖
sudo apt install python3-colcon-common-extensions python3-rosdep

4.2 PX4 与 ROS2 通信节点

下面是一个基础的 ROS2 节点，用于与 PX4 进行通信，获取无人机状态并发送控制指令：

PX4与ROS2通信节点

px4_communication_node.cpp

#include "rclcpp/rclcpp.hpp"
#include "sensor_msgs/msg/imu.hpp"
#include "nav_msgs/msg/odometry.hpp"
#include "geometry_msgs/msg/twist.hpp"
#include "mavros_msgs/msg/state.hpp"
#include "mavros_msgs/srv/command_bool.hpp"
#include "mavros_msgs/srv/set_mode.hpp"
#include "mavros_msgs/srv/command_tol.hpp"

/**
* @brief PX4通信节点，用于获取无人机状态并发送控制指令
*
* 该节点实现以下功能：
* 1. 订阅无人机状态（连接状态、武装状态、飞行模式）
* 2. 订阅IMU数据和里程计数据
* 3. 提供服务客户端用于武装无人机、设置飞行模式和发送起飞指令
* 4. 发布速度控制指令
*/
class PX4CommunicationNode : public rclcpp::Node
{
public:
PX4CommunicationNode() : Node("px4_communication_node")
{
// 初始化状态变量
current_state_.connected = false;
current_state_.armed = false;
current_state_.mode = "OFFBOARD";

// 订阅无人机状态
state_sub_ = this->create_subscription<mavros_msgs::msg::State>(
"mavros/state", 10,
std::bind(&PX4CommunicationNode::state_callback, this, std::placeholders::_1));

// 订阅IMU数据
imu_sub_ = this->create_subscription<sensor_msgs::msg::Imu>(
"mavros/imu/data", 10,
std::bind(&PX4CommunicationNode::imu_callback, this, std::placeholders::_1));

// 订阅里程计数据
odom_sub_ = this->create_subscription<nav_msgs::msg::Odometry>(
"mavros/local_position/odom", 10,
std::bind(&PX4CommunicationNode::odom_callback, this, std::placeholders::_1));

// 发布速度控制指令
vel_pub_ = this->create_publisher<geometry_msgs::msg::Twist>("mavros/setpoint_velocity/cmd_vel_unstamped", 10);

// 创建服务客户端：武装无人机
arming_client_ = this->create_client<mavros_msgs::srv::CommandBool>("mavros/cmd/arming");

// 创建服务客户端：设置飞行模式
set_mode_client_ = this->create_client<mavros_msgs::srv::SetMode>("mavros/set_mode");

// 创建服务客户端：起飞
takeoff_client_ = this->create_client<mavros_msgs::srv::CommandTol>("mavros/cmd/takeoff");

// 等待服务可用
while (!arming_client_->wait_for_service(std::chrono::seconds(1)) &&
!set_mode_client_->wait_for_service(std::chrono::seconds(1)) &&
!takeoff_client_->wait_for_service(std::chrono::seconds(1))) {
if (!rclcpp::ok()) {
RCLCPP_ERROR(this->get_logger(), "用户中断等待服务!");
return;
}
RCLCPP_INFO(this->get_logger(), "等待服务可用...");
}

// 设置控制指令发布周期为10Hz
control_timer_ = this->create_wall_timer(
std::chrono::milliseconds(100),
std::bind(&PX4CommunicationNode::control_loop, this));

RCLCPP_INFO(this->get_logger(), "PX4通信节点初始化完成");
}

/**
* @brief 无人机状态回调函数
*/
void state_callback(const mavros_msgs::msg::State::SharedPtr msg)
{
current_state_ = *msg;
RCLCPP_DEBUG(this->get_logger(), "状态更新: 连接=%d, 武装=%d, 模式=%s",
msg->connected, msg->armed, msg->mode.c_str());
}

/**
* @brief IMU数据回调函数
*/
void imu_callback(const sensor_msgs::msg::Imu::SharedPtr msg)
{
current_imu_ = *msg;
// 可以在这里处理IMU数据，如姿态解算等
}

/**
* @brief 里程计数据回调函数
*/
void odom_callback(const nav_msgs::msg::Odometry::SharedPtr msg)
{
current_odom_ = *msg;
current_position_ = msg->pose.pose.position;
// 可以在这里处理位置数据
}

/**
* @brief 发送武装指令
* @param arm 武装状态，true为武装，false为解除武装
* @return 服务调用结果
*/
bool arm_drone(bool arm)
{
auto request = std::make_shared<mavros_msgs::srv::CommandBool::Request>();
request->value = arm;

auto result_future = arming_client_->async_send_request(request);

// 等待服务响应，超时时间5秒
if (rclcpp::spin_until_future_complete(this->get_node_base_interface(), result_future, std::chrono::seconds(5)) !=
rclcpp::FutureReturnCode::SUCCESS) {
RCLCPP_ERROR(this->get_logger(), "武装服务调用失败");
return false;
}

auto result = result_future.get();
if (result->success) {
RCLCPP_INFO(this->get_logger(), "无人机%s成功", arm ? "武装" : "解除武装");
return true;
} else {
RCLCPP_ERROR(this->get_logger(), "无人机%s失败", arm ? "武装" : "解除武装");
return false;
}
}

/**
* @brief 设置飞行模式
* @param mode 飞行模式字符串，如"OFFBOARD", "AUTO.LOITER", "STABILIZED"等
* @return 服务调用结果
*/
bool set_flight_mode(const std::string &mode)
{
auto request = std::make_shared<mavros_msgs::srv::SetMode::Request>();
request->custom_mode = mode;

auto result_future = set_mode_client_->async_send_request(request);

// 等待服务响应，超时时间5秒
if (rclcpp::spin_until_future_complete(this->get_node_base_interface(), result_future, std::chrono::seconds(5)) !=
rclcpp::FutureReturnCode::SUCCESS) {
RCLCPP_ERROR(this->get_logger(), "设置模式服务调用失败");
return false;
}

auto result = result_future.get();
if (result->mode_sent) {
RCLCPP_INFO(this->get_logger(), "飞行模式设置为: %s", mode.c_str());
return true;
} else {
RCLCPP_ERROR(this->get_logger(), "飞行模式设置失败: %s", mode.c_str());
return false;
}
}

/**
* @brief 发送起飞指令
* @param altitude 起飞高度（米）
* @param latitude 目标纬度（可选，留空则使用当前位置）
* @param longitude 目标经度（可选，留空则使用当前位置）
* @return 服务调用结果
*/
bool takeoff(float altitude, double latitude = 0.0, double longitude = 0.0)
{
auto request = std::make_shared<mavros_msgs::srv::CommandTol::Request>();
request->min_pitch = 0.0;
request->yaw = 0.0;
request->latitude = latitude;
request->longitude = longitude;
request->altitude = altitude;

auto result_future = takeoff_client_->async_send_request(request);

// 等待服务响应，超时时间30秒
if (rclcpp::spin_until_future_complete(this->get_node_base_interface(), result_future, std::chrono::seconds(30)) !=
rclcpp::FutureReturnCode::SUCCESS) {
RCLCPP_ERROR(this->get_logger(), "起飞服务调用失败");
return false;
}

auto result = result_future.get();
if (result->success) {
RCLCPP_INFO(this->get_logger(), "起飞指令发送成功，目标高度: %.2f米", altitude);
return true;
} else {
RCLCPP_ERROR(this->get_logger(), "起飞指令发送失败");
return false;
}
}

/**
* @brief 发送速度控制指令
* @param x 机体坐标系X方向速度（米/秒）
* @param y 机体坐标系Y方向速度（米/秒）
* @param z 机体坐标系Z方向速度（米/秒）
* @param yaw_rate 偏航角速度（弧度/秒）
*/
void send_velocity_command(float x, float y, float z, float yaw_rate = 0.0)
{
geometry_msgs::msg::Twist vel_cmd;
vel_cmd.linear.x = x;
vel_cmd.linear.y = y;
vel_cmd.linear.z = z;
&nbs