【PX4-AutoPilot教程-Offboard】MAVROS功能包控制无人机进入offboard模式飞行官方例程（C++实现）

MAVROS功能包控制无人机进入offboard模式飞行官方例程（C++实现）

• • 准备工作
  • 运行过程
  • 运行过程中可能出现的BUG

主要介绍如何通过MAVROS功能包的offboard模式控制gazebo中的飞机起飞到高度两米。

准备工作

首先建立工作空间，这里建立一个名为uav_demo_ws的工作空间。

mkdir -p uav_demo_ws/src

之后创建功能包，这里命名为offboard_run。

cd uav_demo_ws/src
catkin_create_pkg offboard_run roscpp std_msgs geometry_msgs mavros_msgs

建立C++脚本作为这个功能包的执行文件。

cd offboard_run/src
gedit offboard_run_node.cpp

把官方代码复制到这个文件里面，保存退出。

//一系列头文件

//ros库
#include <ros/ros.h>

//发布的位置消息体对应的头文件，该消息体的类型为geometry_msgs::PoseStamped
//用来进行发送目标位置
/*
ros官网上这样定义
# A Pose with reference coordinate frame and timestamp
Header header
Pose pose
实际上就是一个带有头消息和位姿的消息
*/
#include <geometry_msgs/PoseStamped.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/CommandBool.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/SetMode.h>

//订阅的消息体的头文件，该消息体的类型为mavros_msgs::State
//查看无人机的状态
/*
wiki上是这样的
std_msgs/Header header
bool connected
bool armed
bool guided
bool manual_input
string mode
uint8 system_status

解析如下：
header：消息头，包含时间戳和框架信息；
connected：表示是否连接到了 mavros 节点；
armed：表示无人机当前是否上锁；
guided：表示无人机当前是否处于 GUIDED 模式；
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 current_state;
 
//订阅时的回调函数，接受到该消息体的内容时执行里面的内容，内容是储存飞控当前的状态
void state_cb(const mavros_msgs::State::ConstPtr& msg){
    current_state = *msg;
}

int main(int argc, char **argv)
{
    //ros系统的初始化，argc和argv在后期节点传值会使用，最后一个参数offb_node为节点名称
    ros::init(argc, argv, "offb_node"); 

    //实例化ROS句柄，这个ros::NodeHandle类封装了ROS中的一些常用功能
    ros::NodeHandle nh;

    //这是一个订阅者对象，可以订阅无人机的状态信息（状态信息来源为MAVROS发布），用来储存无人机的状态，在回调函数中会不断更新这个状态变量
    //<>里面为模板参数，传入的是订阅的消息体类型，（）里面传入三个参数，分别是该消息体的位置、缓存大小（通常为1000）、回调函数
    ros::Subscriber state_sub = nh.subscribe<mavros_msgs::State>("mavros/state", 10, state_cb);

    //这是一个发布者对象，用来在本地坐标系下发布目标点，后面会以20Hz频率发布目标点
    //<>里面为模板参数，传入的是发布的消息体类型，（）里面传入两个参数，分别是该消息体的位置、缓存大小（通常为1000）
    ros::Publisher local_pos_pub = nh.advertise<geometry_msgs::PoseStamped>("mavros/setpoint_position/local", 10);

    //一个客户端，用来解锁无人机，这是因为无人机如果降落后一段时间没有收到信号输入，会自动上锁来保障安全
    //启动服务的函数为nh下的serviceClient<>()函数，<>里面是该服务的类型，（）里面是该服务的路径
    ros::ServiceClient arming_client = nh.serviceClient<mavros_msgs::CommandBool>("mavros/cmd/arming");

    //一个客户端，用来切换飞行模式
    //启动服务的函数为nh下的serviceClient<>()函数，<>里面是该服务的类型，（）里面是该服务的路径
    ros::ServiceClient set_mode_client = nh.serviceClient<mavros_msgs::SetMode>("mavros/set_mode");

    //官方要求local_pos_pub发布速率必须快于2Hz，这里设置为20Hz
    //PX4在两个Offboard命令之间有一个500ms的延时，如果超过此延时，系统会将回到无人机进入Offboard模式之前的最后一个模式。
    ros::Rate rate(20.0);

    // 等待飞控和MAVROS建立连接，current_state是我订阅的MAVROS的状态，在收到心跳包之后连接成功跳出循环
    while(ros::ok() && !current_state.connected){
        ros::spinOnce();
        rate.sleep();
    }

    //实例化一个geometry_msgs::PoseStamped类型的对象，并对其赋值，最后将其发布出去
    //尽管PX4在航空航天常用的NED坐标系下操控飞机，但MAVROS将自动将该坐标系切换至常规的ENU坐标系下
    geometry_msgs::PoseStamped pose;
    pose.pose.position.x = 0;
    pose.pose.position.y = 0;
    pose.pose.position.z = 2;

    //在进入Offboard模式之前，必须已经启动了local_pos_pub数据流，否则模式切换将被拒绝。
    //这里的100可以被设置为任意数
    for(int i = 100; ros::ok() && i > 0; --i){
        local_pos_pub.publish(pose);
        ros::spinOnce();
        rate.sleep();
    }

    //建立一个类型为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();

    //大循环，只要节点还在ros::ok()的值就为正
    while(ros::ok())
    {
        //首先判断当前模式是否为offboard模式，如果不是，则进入if语句内部
        //这里是5秒钟进行一次判断，避免飞控被大量的请求阻塞
        if( current_state.mode != "OFFBOARD" && (ros::Time::now() - last_request > ros::Duration(5.0)))
        {
            //客户端set_mode_client向服务端offb_set_mode发起请求call，然后服务端回应response将模式返回，这就打开了offboard模式
            if( set_mode_client.call(offb_set_mode) && offb_set_mode.response.mode_sent)
            {
                //打开Offboard模式后在终端打印信息
                ROS_INFO("Offboard enabled");
            }
            //更新时间
            last_request = ros::Time::now();
        }

        //else指已经为offboard模式，则进入if语句内部
        else 
        {
            //判断当前状态是否解锁，如果没有解锁，则进入if语句内部
            //这里是5秒钟进行一次判断，避免飞控被大量的请求阻塞
            if( !current_state.armed && (ros::Time::now() - last_request > ros::Duration(5.0)))
            {
                //客户端arming_client向服务端arm_cmd发起请求call，然后服务端回应response成功解锁，则解锁成功
                if( arming_client.call(arm_cmd) && arm_cmd.response.success)
                {
                    //解锁后在终端打印信息
                    ROS_INFO("Vehicle armed");
                }
                //更新时间
                last_request = ros::Time::now();
            }
        }

        //发布位置信息，所以综上飞机只有先打开offboard模式然后解锁才能飞起来
        local_pos_pub.publish(pose); 
        
        //当spinOnce函数被调用时，会调用回调函数队列中第一个回调函数，这里回调函数是state_cb函数
        ros::spinOnce();

        //根据前面ros::Rate rate(20.0);制定的发送频率自动休眠 休眠时间 = 1/频率
        rate.sleep();
    }

    return 0;
}

补充内容：

mavros_msgs库中的消息文件可以在以下网址找到（比如State）：

https://docs.ros.org/en/noetic/api/mavros_msgs/html/msg/

mavros_msgs库中的服务文件可以在以下网址找到（比如CommandBool、SetMode）：

https://docs.ros.org/en/noetic/api/mavros_msgs/html/srv/

打开我们创建的offboard_run功能包里面的CMakeLists.txt文件。

cd ..
gedit CMakeLists.txt

在最后添加下面两行代码，然后保存关闭。

add_executable(${PROJECT_NAME}_node src/offboard_run_node.cpp)
target_link_libraries(${PROJECT_NAME}_node
  ${catkin_LIBRARIES}
)

到uav_demo_ws目录下对工作空间进行编译。

cd ~/uav_demo_ws
catkin_make

运行过程

打开一个终端，在PX4源码处启动无人机gazebo仿真。

cd Firmware
make px4_sitl gazebo

新打开另一个终端，运行MAVROS。

roslaunch mavros px4.launch fcu_url:="udp://:14540@127.0.0.1:14557"

再新打开另一个终端，运行ROS节点，这里注意需要先使用source命令配置好运行环境。

cd ~/uav_demo_ws
source ./devel/setup.bash
rosrun offboard_run offboard_run_node

如果觉得每次先使用source命令配置好运行环境麻烦，也可以将环境配置到~/.bashrc中

echo "source ~/uav_demo_ws/devel/setup.bash" >> ~/.bashrc

如果一切顺利，此时在gazebo中飞机已经飞到了两米的高度。

运行过程中可能出现的BUG

在运行过程中，可能会出现一个BUG，就是无人机无法起飞。

PX4源码处无人机gazebo仿真终端显示如下并不断重复。

INFO [commander] Failsafe mode deactivated
INFO [commander] Failsafe mode activated

MAVROS终端显示如下并不断重复。

CMD: Unexpected command 176, result 0

ROS节点终端显示如下并不断重复。

Offboard enabled

这时打开QGC地面站，可以看到无人机处于Not Ready状态。

并且有报错如下。

Failsafe enabled: No manual control stick input

分析原因是因为程序判断没有接入遥控器，认为不安全，所以不允许起飞。

解决方法为：

在前面的三个终端运行的时候，打开QGC地面站，在参数中搜索COM_RCL_EXCEPT。

将COM_RCL_EXCEPT参数改为4并保存。

这时回到gazebo窗口会发现飞机已经正常起飞了。

参考资料：

三、MAVROS功能包的offboard模式控制例子

MAVROS Offboard control example (C++)

如何进行基于ros_gazebo与px4的无人机仿真？

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