基于单目视觉的四旋翼定点降落——如何通过mavros控制pixhawk（二）

    一直拖了这么久没有更新，怪我太懒啦~~~ 今天要特别感谢舒仔仔同学，我的好队友帮我写了这篇博文。这篇文章主要讲了如何通过mavros来控制pixhawk。硬件逻辑是这样子，我们先搞一个odroid（土豪上TX2），再用usb线将odroid与pixhawk连接。所以，odroid发送控制指令来控制pixhawk飞行。pixhawk与外界的通信协议是mavlink。ROS下面有个包叫mavros，说到底就是mavlink的封装，用起来非常简单。话不多说，上正文。

mavros安装：

使用apt-get安装即可：

sudo apt-get install ros-kinetic-mavros ros-kinetic-mavros-extras ros‐kinetic‐control‐toolbox

我所安装的ros版本是kinetic。

其安装过程非常简单，在网上查询使用方法却说得很笼统，大多是按照官方教程，但实际上并没有详细的讲如何使用。

1.  官方例程：

参考：https://dev.px4.io/zh/ros/mavros_offboard.html

在ROS包中创建offb_node.cpp文件（具体怎么建立ROS包，可参考ROS的Wiki教程，写得很详细），并粘贴下面内容：

/**
 * @file offb_node.cpp
 * @brief offboard example node, written with mavros version 0.14.2, px4 flight
 * stack and tested in Gazebo SITL
 */

#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>
#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/SetMode.h>
#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::init(argc, argv, "offb_node");
    ros::NodeHandle nh;

    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");

    //the setpoint publishing rate MUST be faster than 2Hz
    ros::Rate rate(20.0);

    // wait for FCU connection
    while(ros::ok() && current_state.connected){
        ros::spinOnce();
        rate.sleep();
    }

    geometry_msgs::PoseStamped pose;
    pose.pose.position.x = 0;
    pose.pose.position.y = 0;
    pose.pose.position.z = 2;

    //send a few setpoints before starting
    for(int i = 100; ros::ok() && i > 0; --i){
        local_pos_pub.publish(pose);
        ros::spinOnce();
        rate.sleep();
    }

    mavros_msgs::SetMode offb_set_mode;
    offb_set_mode.request.custom_mode = "OFFBOARD";

    mavros_msgs::CommandBool arm_cmd;
    arm_cmd.request.value = true;

    ros::Time last_request = ros::Time::now();

    while(ros::ok()){
        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 {
            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);

        ros::spinOnce();
        rate.sleep();
    }

    return 0;
}

该程序的主要功能是：使无人机缓慢飞到2米的高度。

这是一段复制上去就可以直接用的程序，但想要理解深刻，需要对ROS有一定的了解，在了解后，才能看懂下面的内容。下面将对代码的每一部分进行分析，使其能够实现自己想要实现的功能。

需要注意的是：对于用于发送接收的mavros的结构体变量的使用，最好还是查看mavros包中的变量定义，这有助于灵活的对代码进行调整。

2.     消息的订阅和发布

首先讲述一下以上程序所实现的功能。首先使飞机进入offboard模式，再进行解锁，最后发布指令使飞机飞到2米的高度。这一系列功能的实现就用到了ROS强大的发布订阅机制。

可以参照这两个网页：

http://wiki.ros.org/mavros

https://404warehouse.net/2015/12/20/autopilot-offboard-control-using-mavros-package-on-ros/

这两个网站上都详细说明了mavros可发布和订阅的消息以及变量类型，举例说明：

2.1 消息订阅（注意：所订阅的消息需要有回调函数去接收它）：

可以看到截图中清楚地写明了订阅消息所需要地信息，那为什么这里是发布呢？这是因为这是站在mavros的角度看的呀~

//回调函数
mavros_msgs::State current_state;
void state_cb(const mavros_msgs::State::ConstPtr& msg){
    current_state = *msg;
}

//消息订阅
ros::Subscriber state_sub = nh.subscribe <mavros_msgs::State> ("mavros/state", 10, state_cb); 

2.2 消息发布：

ros::Publisher local_pos_pub = nh.advertise<geometry_msgs::PoseStamped>
            ("mavros/setpoint_position/local", 10);
geometry_msgs::PoseStamped pose;
pose.pose.position.x = 0;
pose.pose.position.y = 0;
pose.pose.position.z = 2;

//消息发布
local_pos_pub.publish(pose);

同理，在下面截图中也有非常清楚的说明：

上面的代码意思是将坐标从初始点飞到2米的高度，同时该结构体中还有另外4个给定姿态的量，就是四元数，x，y，z，w。从下面这张图可以看出：

使用方法嘛~~~

pose.pose.quaternion.x = 0;
pose.pose.quaternion.y = 0;
pose.pose.quaternion.z = 0;
pose.pose.quaternion.w = 0;

想好你的姿态，换算成四元数复制进去就好啦~

所以！mavros的维基是入手mavros控制的最好的教程，不然看其它的都没有用，当然首先还是要掌握ROS的发布订阅机制才行。

发布消息的时候有一点需要注意：

local_pos_pub.publish(pose);

都看得出来，这一句是发布消息的。那是不是有人问，怎么一段代码里发布了这么多遍？

这在上面的链接里也写得很清楚，我们所使用的pixhawk需要以至少2Hz的频率接收消息，否则将会从offboard模式切回RTL模式。

开始的一段，是为了告诉pixhawk有消息要传过来啦~

中间在切换状态和arm的时候，是为了保持其pixhawk接收寄存器里有值，通俗点嘛，就是既然我要用offboard模式了，你总得先告诉我要怎么搞吧，不然刚切过去那会怎么办呢？随意嘛？哈哈~

那当其正常运行的时候，当然要通过发布消息来尽情的控制你的小飞机啦~

3 怎么和pixhawk连接？

Emm，很简单，直接用USB就可以哈哈~

新打开一个终端，运行：

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

再把上面offboard的节点运行起来就可以啦~

当然，要想搞实物，需要先仿真，下面一节我们就来讲讲仿真怎么搞~

参考文献：

1.    http://wiki.ros.org/mavros

2.    https://404warehouse.net/2015/12/20/autopilot-offboard-control-using-mavros-package-on-ros/