mavros笔记（一）：mavros概述与offboard例程解析

前言

最近在准备无人机比赛，使用的是px4，但是在学习mavros的过程中碰到很多问题，尤其是现在网上信息各种混杂，难以找到有效信息（现在CSDN上绝大多数mavros相关博客都是讲一下offboard例程），我便自己记录一下，便于其他人学习，本教程不局限于offboard的基础操作，而是会扩展到很多其他方面（如多重控制、降落等）
我们先介绍一下mavlink：MAVLink是一种轻量级的通信协议，主要用于在无人机和地面站之间进行通信，包含了许多无人机相关的信息和命令，例如无人机的状态、传感器数据、电池电量等等。
大家可以理解为这是一种不占用太多资源的通信语言，可以让无人机接受指令并且执行。
但是我们也不可能去专门学习这个通信协议，这对于专注研究算法的人员来说是毫无必要的，所以相关的开发人员就对mavlink进行封装，使得开发px4的人员可以使用ros命令来控制无人机，大大简化了开发难度。

Mavros

我们这里假定阅读者已经具备了良好的ros基础，后续我们不会对一些基础的ros语法进行详细说明

MAVROS是一个开源的ROS包，用于将ROS和MAVLink协议连接起来，以实现ROS与无人机之间的通信和控制。

MAVROS提供了一个ROS接口，让用户可以方便地访问无人机的状态和传感器数据，并且可以通过ROS话题或服务来控制无人机的飞行和导航。用户可以使用MAVROS来开发各种类型的应用程序，例如自主飞行、航迹规划、图像识别、无人机编队等等。

此外，MAVROS还支持多种无人机的硬件平台，例如Pixhawk、PX4、ArduPilot等，使得用户可以在不同的无人机平台上使用相同的ROS接口，我们这里使用PX4作为无人机平台。

官方例程

我们先基于官方的offboard例程，来看一下官方是如何使用mavros控制无人机飞行的
官方例程的思想很简单
1.初始化一系列需要的对象和数据
2.给飞控发送一百个数据，用于激活（因为想转到OFFBOARD模式需要先发送一百次数据）
3.发送模式设置请求，如果模式设置通过，则打印信息：OFFBOARD
我们使用流程图展示一下
首先是外层

Created with Raphaël 2.3.0 

 开始：初始化一系列对象 

 链接正常? 

 初始化设置 

 ros正常运行 

 进入飞行主循环 

 yes 

 no 

 yes 

主循环里面是这样的

Created with Raphaël 2.3.0 

 ros运行正常 

 是否offboard模式 

 是否解锁 

 发送目标点 

 隔五秒尝试解锁 

 隔五秒尝试切换 

 yes 

 no 

 yes 

 no 

然后官方给的C++版本的例程是这样的

//首先导入一系列头文件#include<ros/ros.h>//ros库#include<geometry_msgs/PoseStamped.h>//发布的位置消息体对应的头文件，该消息体的类型为geometry_msgs：：PoseStamped//用来进行发送目标位置#include<mavros_msgs/CommandBool.h>//CommandBool服务的头文件，该服务的类型为mavros_msgs：：CommandBool//用来进行无人机解锁#include<mavros_msgs/SetMode.h>//SetMode服务的头文件，该服务的类型为mavros_msgs：：SetMode//用来设置无人机的飞行模式，切换offboard#include<mavros_msgs/State.h>//订阅的消息体的头文件，该消息体的类型为mavros_msgs：：State//查看无人机的状态//建立一个订阅消息体类型的变量，用于存储订阅的信息
mavros_msgs::State current_state;//订阅时的回调函数，接受到该消息体的内容时执行里面的内容，这里面的内容就是赋值voidstate_cb(const mavros_msgs::State::ConstPtr& msg){
    current_state =*msg;}intmain(int argc,char**argv){
    ros::init(argc, argv,"offb_node");//ros系统的初始化，最后一个参数为节点名称
    ros::NodeHandle nh;//订阅。<>里面为模板参数，传入的是订阅的消息体类型，（）里面传入三个参数，分别是该消息体的位置、缓存大小（通常为1000）、回调函数
    ros::Subscriber state_sub = nh.subscribe<mavros_msgs::State>("mavros/state",10, state_cb);//发布之前需要公告，并获取句柄，发布的消息体的类型为：geometry_msgs::PoseStamped
    ros::Publisher local_pos_pub = nh.advertise<geometry_msgs::PoseStamped>("mavros/setpoint_position/local",10);//启动服务1，设置客户端（Client）名称为arming_client，客户端的类型为ros::ServiceClient，//启动服务用的函数为nh下的serviceClient<>()函数，<>里面是该服务的类型，（）里面是该服务的路径
    ros::ServiceClient arming_client = nh.serviceClient<mavros_msgs::CommandBool>("mavros/cmd/arming");//启动服务2，设置客户端（Client）名称为set_mode_client，客户端的类型为ros::ServiceClient，//启动服务用的函数为nh下的serviceClient<>()函数，<>里面是该服务的类型，（）里面是该服务的路径
    ros::ServiceClient set_mode_client = nh.serviceClient<mavros_msgs::SetMode>("mavros/set_mode");//the setpoint publishing rate MUST be faster than 2Hz
    ros::Rate rate(20.0);// 等待飞控连接mavros，current_state是我们订阅的mavros的状态，连接成功在跳出循环while(ros::ok()&&!current_state.connected){
        ros::spinOnce();
        rate.sleep();}//先实例化一个geometry_msgs::PoseStamped类型的对象，并对其赋值，最后将其发布出去
    geometry_msgs::PoseStamped pose;
    pose.pose.position.x =0;
    pose.pose.position.y =0;
    pose.pose.position.z =2;//建立一个类型为SetMode的服务端offb_set_mode，并将其中的模式mode设为"OFFBOARD"，作用便是用于后面的//客户端与服务端之间的通信（服务）
    mavros_msgs::SetMode offb_set_mode;
    offb_set_mode.request.custom_mode ="OFFBOARD";//建立一个类型为CommandBool的服务端arm_cmd，并将其中的是否解锁设为"true"，作用便是用于后面的//客户端与服务端之间的通信（服务）
    mavros_msgs::CommandBool arm_cmd;
    arm_cmd.request.value =true;//更新时间
    ros::Time last_request = ros::Time::now();while(ros::ok())//进入大循环{//首先判断当前模式是否为offboard模式，如果不是，则客户端set_mode_client向服务端offb_set_mode发起请求call，//然后服务端回应response将模式返回，这就打开了offboard模式if( current_state.mode !="OFFBOARD"&&(ros::Time::now()- last_request > ros::Duration(5.0))){if( set_mode_client.call(offb_set_mode)&& offb_set_mode.response.mode_sent){ROS_INFO("Offboard enabled");//打开模式后打印信息}
            last_request = ros::Time::now();}else//else指已经为offboard模式，然后进去判断是否解锁，如果没有解锁，则客户端arming_client向服务端arm_cmd发起请求call//然后服务端回应response成功解锁，这就解锁了{if(!current_state.armed &&(ros::Time::now()- last_request > ros::Duration(5.0))){if( arming_client.call(arm_cmd)&& arm_cmd.response.success){ROS_INFO("Vehicle armed");//解锁后打印信息}
                last_request = ros::Time::now();}}
 
        local_pos_pub.publish(pose);//发布位置信息，所以综上飞机只有先打开offboard模式然后解锁才能飞起来
 
        ros::spinOnce();
        rate.sleep();}return0;}

接下来我们进行更详细的分析

1.头文件分析：各种消息

#include<ros/ros.h>//ros库#include<geometry_msgs/PoseStamped.h>//发布的位置消息体对应的头文件，该消息体的类型为geometry_msgs：：PoseStamped//用来进行发送目标位置/*
ros官网上这样定义
# A Pose with reference coordinate frame and timestamp
Header header
Pose pose
实际上就是一个带有头消息和位姿的消息
*/#include<mavros_msgs/CommandBool.h>/*
CommandBool服务的头文件，该服务的类型为mavros_msgs：：CommandBool
其结构如下（来源于ros wiki）
# Common type for switch commands

bool value
---
bool success
uint8 result

可以看到，发送的请求是一个bool类型的数据，为True则解锁，为False则上锁
返回的响应中
success是一个bool类型的参数，表示上电/断电操作是否成功执行。
如果操作成功执行，success值为True，否则为False。
result是一个int32类型的参数，表示执行上电/断电操作的结果。
如果解锁/上锁操作成功执行，result值为0，
否则为其他值，表示执行解锁/上锁操作时发生了某种错误或异常。可以根据这个数值查看是哪种问题导致
*///用来进行无人机解锁#include<mavros_msgs/SetMode.h>//SetMode服务的头文件，该服务的类型为mavros_msgs：：SetMode//用来设置无人机的飞行模式，切换offboard/*
wiki上的消息定义如下
# set FCU mode
#
# Known custom modes listed here:
# http://wiki.ros.org/mavros/CustomModes

# basic modes from MAV_MODE
uint8 MAV_MODE_PREFLIGHT = 0
uint8 MAV_MODE_STABILIZE_DISARMED = 80
uint8 MAV_MODE_STABILIZE_ARMED = 208
uint8 MAV_MODE_MANUAL_DISARMED = 64
uint8 MAV_MODE_MANUAL_ARMED = 192
uint8 MAV_MODE_GUIDED_DISARMED = 88
uint8 MAV_MODE_GUIDED_ARMED = 216
uint8 MAV_MODE_AUTO_DISARMED = 92
uint8 MAV_MODE_AUTO_ARMED = 220
uint8 MAV_MODE_TEST_DISARMED = 66
uint8 MAV_MODE_TEST_ARMED = 194

uint8 base_mode # filled by MAV_MODE enum value or 0 if custom_mode != ''
string custom_mode # string mode representation or integer
---
bool success

实际上String类型的变量custom_mode就是我们想切换的模式，有如下选择
MANUAL，ACRO，ALTCTL，POSCTL，OFFBOARD，STABILIZED，RATTITUDE，AUTO.MISSION
AUTO.LOITER，AUTO.RTL，AUTO.LAND，AUTO.RTGS，AUTO.READY，AUTO.TAKEOFF
*/#include<mavros_msgs/State.h>//订阅的消息体的头文件，该消息体的类型为mavros_msgs：：State//用于描述无人机当前状态的各种参数/*
wiki上是这样的
std_msgs/Header header
bool connected
bool armed
bool guided
bool manual_input
string mode
uint8 system_status
PS：后面还有一堆描述无人机状态的，我这里并没有写，因为一般用不到，感兴趣的可以去wiki上看
http://docs.ros.org/en/noetic/api/mavros_msgs/html/msg/State.html
解析如下：
header：消息头，包含时间戳和框架信息；
connected：表示是否连接到了 mavros 节点；
armed：表示无人机当前是否上锁；
guided：表示无人机当前是否处于 GUIDED 模式；
mode：表示当前无人机所处的模式，包括以下几种：
*/

2.流程分析

首先，offboard例程在开始的时候初始化了一系列话题和服务对象，用来进行各种设置和飞行控制

//这是一个订阅者对象，可以订阅无人机的状态信息（状态信息来源为mavros发布），用来判断无人机的状态//程序在最开始的时候声明了一个全局变量，用来存储无人机状态，在回调函数里面会不断更新这个状态变量
    ros::Subscriber state_sub = nh.subscribe<mavros_msgs::State>("mavros/state",10, state_cb);//用来在本地坐标系下发布目标点
    ros::Publisher local_pos_pub = nh.advertise<geometry_msgs::PoseStamped>("mavros/setpoint_position/local",10);//一个客户端，用来解锁无人机，这是因为无人机如果降落后一段时间没有收到信号输入，会自动上锁来保障安全//所以如果想让无人机飞行，必须使用这个实现解锁
    ros::ServiceClient arming_client = nh.serviceClient<mavros_msgs::CommandBool>("mavros/cmd/arming");//因为无人机有多种飞行模式，所以需要程序运行时进行切换
    ros::ServiceClient set_mode_client = nh.serviceClient<mavros_msgs::SetMode>("mavros/set_mode");
    ros::Rate rate(20.0);//因为无人机在空中飞行，更难以控制，所以要求信号的频率较高// 等待飞控连接mavros，current_state是我们订阅的mavros的状态，连接成功在跳出循环while(ros::ok()&&!current_state.connected){
        ros::spinOnce();
        rate.sleep();}//大家还记得头文件里面mavros_msgs/State.h吗？这个消息格式里面有很多属性可以说明无人机的状态//加上我们创建一个全局变量来不断监视无人机状态，在这里我们就可以查看无人机的连接状态

未完待续