MAVROS机外（offboard）控制例程

PX4与ROS各部分的关系

Simulator仿真器（Gazebo）：模拟真实飞行，即模拟计算出真实飞行时的传感器状态，包括GPS，IMU（惯性测量单元，对姿态进行精密位移推算）等，将这些信息发送给飞控。仿真器包含了传感器和执行机构的模型，负责产生传感器的原始数据和执行最后的底层控制指令。
PX4到仿真器：PX4给仿真器只会发送一个HIL_ACTUATOR_CONTROLS的MAVLink消息，PX4固件本身则运行控制及估计模块（即飞控核心算法）。

端口号
UDP端口 14540 用于offboard模式的通讯。 Offboard APIs are expected to listen for connections on this port.
UDP端口 14550 用地面站的通讯。
TCP端口 4560 用于与仿真器的通讯。

Gazebo仿真
PX4固件中有一个文件夹Firmware/Tools/sitl_gazebo，下面这3个文件夹分别对应修改模型、修改模型参数、修改世界。
src文件夹。这里是核心代码，即plugin（插件）代码。比如gps、imu、电机的插件，简单说如何模拟产生gps信号，就是这里的代码负责的。Gazebo自身也会提供一些默认插件，或者第三方也会提供插件，比如你要新增一个传感器，就可以找到相应的插件。
models文件夹。存放各种Gazebo模型文件。
worlds文件夹。存放Gazebo的世界环境文件。

http://di3renlei.cn/info/3823
**代码详细注释：**转载于https://blog.csdn.net/sinat_16643223/article/details/107856644

#include <ros/ros.h>
#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
#include <mavros_msgs/State.h>  //订阅的消息体的头文件，该消息体的类型为mavros_msgs：：State
 
//建立一个订阅消息体类型的变量，用于存储订阅的信息
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系统的初始化，最后一个参数为节点名称
    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();
    }
 
    return 0;
}

图文教程：https://blog.csdn.net/sinat_16643223/article/details/107136895?utm_medium=distribute.pc_relevant.none-task-blog-baidujs_title-4&spm=1001.2101.3001.4242
通过MAVROS在Gazebo中应用于Iris四旋翼上。在教程最后，无人机缓慢起飞到高度2米。

视频教程：https://www.bilibili.com/video/BV15A411g7ra/?spm_id_from=333.788.recommend_more_video.-1

//建立工作区
mkdir  offboard_01
cd offboard_01/
mkdir src
cd src/
catkin_init_workspace

打开roboware中刚刚建立的工作区，在src下面新建ros包。roboware操作教程：https://zhuanlan.zhihu.com/p/151031343
编辑依赖的ROS包列表，一般添加roscpp,rospy,std_msgs，根据自己实际需要进行填写。
这里新建ros包为：offboard roscpp rospy std_msgs ，ros包的名字就叫做offboard。
在ros包下面的src中添加cpp文件，这里取名叫做hello（名字不要叫test,否则可能会引起其他文件冲突）。具体操作见roboware教程的6 7步骤。然后将上面的代码复制进去进行debug和编译。等待编译完成后，如果你要在gazebo中仿真，在PX4目录下运行命令

make px4_sitl gazebo

打开QGC
使用如下命令启动MAVROS:roslaunch mavros px4.launch fcu_url:="udp://:14540@127.0.0.1:14557"
再打开一个新的终端：

~$ source ~/offboard_01/devel/setup.bash//用source刷新环境变量
~$ rosrun offboard hello

不用roboware:https://www.it610.com/article/1280886071940366336.htm
MAVROS功能包的offboard模式控制例子
catkin_make编译见https://blog.csdn.net/hhaowang/article/details/101691986
进入gazebo中进行观察