MAVROS 安装配置与 ROS C++ 仿真实战指南

MAVROS 简介

MAVROS 是无人机开发中连接机器人操作系统（ROS）与飞控系统的关键中间件。它基于 MAVLink 轻量级通信协议，为 ROS 生态提供了与 Pixhawk、ArduPilot、PX4 等飞控通信的统一接口。开发者无需深入底层飞控协议，即可直接调用 RViz 可视化、Gazebo 仿真及导航栈等 ROS 工具链，快速搭建任务。

其核心功能在于双向数据传输：一方面订阅飞控的 IMU、GPS、电池状态等传感器数据并发布至 ROS 话题；另一方面接收 ROS 指令（如起飞、降落），转换为 MAVLink 消息发送至飞控执行。此外，MAVROS 支持串口、UDP、TCP 等多种通信接口，并提供参数配置服务，允许动态读写飞控参数。

MAVROS 安装

在 Ubuntu 环境下，通过以下命令安装基础包和扩展包：

sudo apt-get install ros-noetic-mavros
sudo apt install ros-noetic-mavros-extras

地理计算库 GeographicLib 用于处理高精度地理坐标转换（如 WGS84 到 ENU）。MAVROS 依赖此库进行导航解算，需手动下载数据集：

wget https://raw.githubusercontent.com/mavlink/mavros/master/mavros/scripts/install_geographiclib_datasets.sh
sudo chmod +x ./install_geographiclib_datasets.sh
sudo ./install_geographiclib_datasets.sh

MAVROS 基础知识

坐标系说明

MAVROS 涉及多个坐标系，理解它们对调试至关重要：

• global 系：通常由 GPS 定义，即经纬高坐标系（WGS84）。
• local 系：
  • ROS 端：采用 ENU 坐标系（东 - 北 - 天），原点通常是 MAVROS 首次收到里程计信息时飞控的位置。x 轴朝东，y 轴朝北，z 轴朝天。
  • 飞控端：PX4/ArduPilot 内部常用 NED 坐标系（北 - 东 - 地）。
• body 系：
  • ROS 端：FLU 坐标系（前 - 左 - 上），对应 base_link。
  • 飞控端：FRD 坐标系（前 - 右 - 下）。

实际场景中，map 系与 Gazebo 坐标系方向一致。机身前方定义为东，即 map 系的 x 轴正方向，此时 ROS 端的 map 系与 base_link 系方向基本对齐。

常用话题

/mavros/state

订阅 MAVROS 的状态数据，包括连接状态、是否解锁及当前模式。

• connected：物理连接状态。
• armed：电机是否解锁（true 表示可飞行）。
• mode：飞行模式，如 MANUAL（手动）、POSCTL（位置控制）、OFFBOARD（板外模式）、MISSION（任务）等。
• system_status：系统整体状态（未初始化、待机、紧急等）。

数据类型：mavros_msgs/State

/mavros/setpoint_position/local

用于定点飞行控制。注意该话题仅生效 position 字段，不控制姿态 orientation。坐标基于 ROS 端 local 坐标系。

数据类型：geometry_msgs/PoseStamped

/mavros/local_position/odom

提供无人机位姿和速度里程计信息。位姿相对于 ROS local 系，速度相对于 body 系。

数据类型：nav_msgs/Odometry

/mavros/setpoint_raw/local

主要用于设置加速度或推力。需注意 coordinate_frame 通常设为 FRAME_LOCAL_NED，MAVROS 会自动进行坐标变换。type_mask 用于忽略特定通道（如只控制加速度则忽略位置和速度）。

数据类型：mavros_msgs::PositionTarget

/mavros/setpoint_raw/attitude

设置无人机的姿态、推力和各轴角速度。

数据类型：mavros_msgs::AttitudeTarget

常用服务

/mavros/cmd/arming

用于解锁或锁定电机。

数据类型：mavros_msgs/CommandBool (value: true 解锁，false 锁定)

/mavros/set_mode

切换飞控飞行模式。ROS 控制通常需要切换到 OFFBOARD 模式。

数据类型：mavros_msgs/SetMode (custom_mode: "OFFBOARD")

仿真案例

案例一：设置 Offboard 模式并解锁

在代码初始化阶段，我们需要建立与服务节点的连接，并在循环中检查状态。这里要注意，只有当状态机确认进入 Offboard 模式后，才能发送控制指令。

this->arm_client_ = this->nh_.serviceClient<mavros_msgs::CommandBool>("mavros/cmd/arming");
this->set_mode_client_ = this->nh_.serviceClient<mavros_msgs::SetMode>("mavros/set_mode");

mavros_msgs::SetMode offboardMode;
offboardMode.request.custom_mode = "OFFBOARD";
mavros_msgs::CommandBool armCmd;
armCmd.request.value = true;

ros::Time lastRequest = ros::Time::now();
while(ros::ok()) {
    if(this->mavros_state_.mode != "OFFBOARD" && (ros::Time::now() - lastRequest > ros::Duration(5.0))) {
        if(this->set_mode_client_.call(offboardMode) && offboardMode.response.mode_sent) {
            cout << "Offboard mode enabled." << endl;
        }
        lastRequest = ros::Time::now();
    } else {
        if(!this->mavros_state_.armed && (ros::Time::now() - lastRequest > ros::Duration(5.0))) {
            if(this->arm_client_.call(armCmd) && armCmd.response.success) {
                cout << "Vehicle armed." << endl;
            }
            lastRequest = ros::Time::now();
        }
    }
    r.sleep();
}

案例二：无人机起飞到指定高度

假设需要飞到 1 米高度，我们发布位置目标。注意 frame_id 应设置为 map，确保坐标系一致。

this->position_pub_ = this->nh_.advertise<geometry_msgs::PoseStamped>("/mavros/setpoint_position/local", 1000);
geometry_msgs::PoseStamped ps;
ps.header.frame_id = "map";
ps.header.stamp = ros::Time::now();
ps.pose.position.x = this->odom_.pose.pose.position.x;
ps.pose.position.y = this->odom_.pose.pose.position.y;
ps.pose.position.z = 1.0; // 目标高度
ps.pose.orientation = this->odom_.pose.pose.orientation;
this->position_pub_.publish(ps);

案例三：获取位姿并更新状态

订阅里程计话题，解析位置和速度信息。这里利用 Eigen 库将四元数转换为旋转矩阵，以便将机体坐标系下的速度转换到世界坐标系或其他需要的参考系。

this->odom_sub_ = this->nh_.subscribe<nav_msgs::Odometry>("/mavros/local_position/odom", 1000, &flightBase::odomCB, this);

void flightBase::odomCB(const nav_msgs::Odometry::ConstPtr &odom) {
    this->odom_ = *odom;
    this->curr_position_(0) = this->odom_.pose.pose.position.x;
    this->curr_position_(1) = this->odom_.pose.pose.position.y;
    this->curr_position_(2) = this->odom_.pose.pose.position.z;

    Eigen::Vector3d curr_vel_body(this->odom_.twist.twist.linear.x,
                                  this->odom_.twist.twist.linear.y,
                                  this->odom_.twist.twist.linear.z);
    Eigen::Vector4d orientation_quat(this->odom_.pose.pose.orientation.w,
                                     this->odom_.pose.pose.orientation.x,
                                     this->odom_.pose.pose.orientation.y,
                                     this->odom_.pose.pose.orientation.z);
    Eigen::Matrix3d orientation_rot = quat2RotMatrix(orientation_quat);
    this->curr_vel_ = orientation_rot * curr_vel_body;
}