基于单目视觉的四旋翼定点降落——如何经过mavros控制pixhawk（二）
一直拖了这么久没有更新，怪我太懒啦~~~ 今天要特别感谢舒仔仔同窗，个人好队友帮我写了这篇博文。这篇文章主要讲了如何经过mavros来控制pixhawk。硬件逻辑是这样子，咱们先搞一个odroid（土豪上TX2），再用usb线将odroid与pixhawk链接。因此，odroid发送控制指令来控制pixhawk飞行。pixhawk与外界的通讯协议是mavlink。ROS下面有个包叫mavros，说到底就是mavlink的封装，用起来很是简单。话很少说，上正文。
html
mavros安装：node
使用apt-get安装便可：函数
sudo apt-get install ros-kinetic-mavros ros-kinetic-mavros-extras ros‐kinetic‐control‐toolbox
我所安装的ros版本是kinetic。网站
其安装过程很是简单，在网上查询使用方法却说得很笼统，大可能是按照官方教程，但实际上并无详细的讲如何使用。url
1. 官方例程：spa
参考：https://dev.px4.io/zh/ros/mavros_offboard.html.net
在ROS包中建立offb_node.cpp文件（具体怎么创建ROS包，可参考ROS的Wiki教程，写得很详细），并粘贴下面内容：code
/**
* @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米的高度。htm
这是一段复制上去就能够直接用的程序，但想要理解深入，须要对ROS有必定的了解，在了解后，才能看懂下面的内容。下面将对代码的每一部分进行分析，使其可以实现本身想要实现的功能。blog
须要注意的是：对于用于发送接收的mavros的结构体变量的使用，最好仍是查看mavros包中的变量定义，这有助于灵活的对代码进行调整。
2. 消息的订阅和发布
首先讲述一下以上程序所实现的功能。首先使飞机进入offboard模式，再进行解锁，最后发布指令使飞机飞到2米的高度。这一系列功能的实现就用到了ROS强大的发布订阅机制。
能够参照这两个网页：
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的节点运行起来就能够啦~
固然，要想搞实物，须要先仿真，下面一节咱们就来说讲仿真怎么搞~
参考文献：
2. https://404warehouse.net/2015/12/20/autopilot-offboard-control-using-mavros-package-on-ros/