control.md · 陆议/Robotics

# ROS-PX4无人机仿真教程（二）：简单飞行控制

作者：小陆

前言：大家好，我是小陆，这里给大家带来一套 ros 仿真无人机的教程。
 
本套教程适用于有 ros 基础的学习者，我们使用的是 px4 固件，教程内容会设计到仿真飞机的搭建，仿真自主飞行，仿真 slam 建图，以及实机的相应操作，后面教程会陆续更新。

本篇教程设计工作空间初始化，编程代码的创建以及详细分析，让大家可以简单的自主操作无人机执行飞行任务。

工作空间初始化

1.在 ubuntu 中打开命令行，在命令提示符下输入：

```
mkdir -p ~/ll_ws/src
cd ~/ll_ws/src
```

该命令为创建工作空间文件夹，其中 ll_ws 是工作空间名称，也可以自定义命名。

文件夹创建好后，可以输入以下命令来初始化工作空间 ：

```
catkin_init_workspace

```
终端回显一个编译文件已经写入，这时返回到上级目录 ll_ws ，输入 catkin_make 即可进行编译。

如图，若终端显示编译成功，此时工作空间中会多出 build 和 devel 文件夹。

然后进入 src 创建我们的第一个功能包。

```
cd src
catkin_create_pkg square geometry_msgs std_msgs mavros roscpp

```

这里 catkin_create_pkg 命令是创建功能包，后面加入了四个依赖项，其中 geometry_msgs 包含了坐标系与时间戳信息，std_msgs 为基础消息构造包， mavros 为具有 MAVLink 通信协议的各种自动驾驶仪提供通信驱动包，roscpp 为 ros C++实现包，具体 msgs 的格式可在 ros wiki 中查看： http://wiki.ros.org/geometry_msgs

square 是功能包的名字，这个功能包目的是使无人机在仿真环境中起飞并飞出一个方形轨迹，所以我将它起名为 square ，大家也可以自定义。

如图功能包创建完毕，这里依然需要回到 ll_ws 目录 输入 catkin_make 再次编译一遍。

编译成功后就可以在功能包中实现我们的无人机飞行代码了。

```
#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;
}

```

以上代码是 px4 官网自带的例程，这个程序流程是让飞机进入 offboard 模式，解锁，之后起飞到 （0，0，2）的世界坐标位置。
我们先将这份代码拷贝下来命名为 control.cpp 放到 ~/ll_ws/src/square/src 中，编译运行，也方便之后的讲解以及代码修改。

文件放到路径后，还需要需在 square 中的 CMakeLists.txt 文件添加下面这句话：

```
add_executable(${PROJECT_NAME}_node src/control.cpp)

```

保存，之后命令行回到 ll_ws 目录并用 catkin_make 编译。

编译后在 ll_ws 目录下命令行输入 source devel/setup.bash 即可在当前终端使用功能包。

或者这里有个更方便的方法，即返回主目录，打开命令行输入：

```
echo source ~/ll_ws/devel/setup.bash >> .bashrc

source .bashrc

```

再使用命令行 在主目录下执行 source .bashrc 即可更方便的通过命令行使用我们的功能包。

那么现在就可以测试飞行代码了 ！ 启动上一章修改好的 launch 文件：

```
roslaunch px4 mavros_posix_stil.launch

```

然后在新的终端中输入

```
rosrun square square_node

```

等待一段时间后，即可看见飞机自动起飞，并且成功飞到了 2m 高的位置。

### 代码详解

现在让我们一起来看看官方的例程代码。

首先是引用包：

```

#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 中的几个头文件，分别是 CommandBool (无人机指令)，SetMode (无人机飞行模式)， 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::NodeHandle nh;

```
其中 mavros/state 关联无人机的状态， mavros/setpoint_position/local 发布符合消息格式的位置信息后，无人机会自动前往指定的地点。

mavros/cmd/arming 用于无人机解锁，还有 mavros/set_mode 设置无人机模式。

官方这份代码是在无人机机启动前发布了无人机指向的指令。

```

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();
    }

```

通过 pose 将无人机的三个坐标全部设置好，用 for 循环发布指令。这个地地点如果仅仅发布一次的话，无人机实机未为到达该地点就会与其他飞行地点指令发生冲突，从而飞行混乱

```

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) && //是否处于 offboard 的模式
                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;
}

```
之后的代码为将无人机设置为offboard版外模式。 解锁无人机。 并且通过 current_state.mode 判断无人机当前所处的状态。 如果确认已经转换到了 "offboard"，则在控制台显示 "Offboard enabled" 。 后面也如此，判断无人机是否解锁。然后循环发布设置的位置指令。

好啦，那现在我们可以通过同样的发布位置指令来让我们的无人机画出一个方形了。