无人机专题（五）控制仿真无人机飞行

zelon1234

接下来就是仿真无人机的控制，这里我们给出了两种基本的方式，分别是地面站控制和Mavros外部例程控制。
在这之前，首先我们需要先配置Gazebo仿真环境。
cd px4_src/PX4-Autopilot     # 进入目录
# 添加环境变量和路径
source Tools/setup_gazebo.bash $(pwd) $(pwd)/build/px4_sitl_default
export ROS_PACKAGE_PATH=$ROS_PACKAGE_PATH:$(pwd):$(pwd)/Tools/sitl_gazebo然后在其中的launch文件下运行单个无人机的launch文件，即可出现熟悉的小飞机了。
cd launch
roslaunch posix_sitl.launch

（一）地面站控制

首先，我们打开地面站，这时刚刚加载的单无人机模型会自动连接上地面站，并在图中显示无人机的模样。


首先测试一下，我们点击图中左上角的“Takeoff”命令，下方会出现一个滑动确认窗口，滑动后你会发现上方Gazebo中的小飞机就飞起来了，这表明一切正常。


接下来，我们来用地面站给无人机发送航点信息的形式来控制无人机飞行。（首先让无人机恢复初始状态，已经原地起飞的可以点击窗口左上角的“Land”按钮让无人机降落）
（1）点击左上角“Plan”进入编辑界面；
（2）点击左上角“Takeoff”，添加起飞命令；


（3）点击左上角“Waypoint”，添加航点（在地图上“随便”点一个点，也别太随便，不然一会儿无人机飞太远找不着了）


（4）点击上方“Upload”更新命令，并点击左上角“Fly”返回初始界面。
（5）最后，我们只需要点击左上角的“Action”按钮，并一路确定，即可让无人机起飞，并飞向我们预先标定的航路点了。


当然，你也可以在设置航路点的时候多添加几个，这样无人机也可以按照你给定的航路点顺序依次完成相应点的飞行，是不是so easy呢。
（二）Mavros外部例程控制

基于地面站的航路点控制只能说是让大家体验一下，入个门，为了后续的开发，我们必须得掌握通过代码来控制无人机飞行，这里我先给出PX4官网对代码的罗列和解释，喜欢看原文的同学可以查看官网的解释。（当然在实际过程中，大家也可以打开地面站，对无人机的飞行姿态进行监控）
在这一部分，需要大家首先掌握ROS开发的基本知识，这里是链接。
这里我主要对步骤进行一个总结：
（1）首先你得先建立ROS的工作空间，并建立一个off_board的功能包（这里大家可以参考ros官网的创建方式，就不在赘述）
（2）在功能包的src目录下新建“off_board_node.cpp”文件，并将以下的官方控制例程复制进去，保存即可。
#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, "off_board_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;
}（3）配置CMakeLists.txt文件，在末尾添加生成可执行文件，保存并退出，然后进行编译。
add_executable(off_board_node src/off_board_node.cpp)
target_link_libraries(off_board_node ${catkin_LIBRARIES})（4）按照本文一开始的方式启动Gazebo仿真，并且在终端启动Mavros，使其连接至飞控。
roslaunch mavros px4.launch fcu_url:="udp://:14540@127.0.0.1:14557"（5）运行刚刚的节点，就可以看到仿真无人机飞起到2米左右的高度悬停。
关于这段代码的详细解释大家可以去官网详细了解一下。小无人机主要是通过下图的数据进行航点的控制，当我们想改变无人机飞的点时，在pose这个信息被pub出去之前改变他就可以了。


当然，这段代码只是一个测试例程，帮助大家学习和了解的，这里有很多是冗余的甚至是和实际自主飞行不相符的步骤（比如在实际飞行中我们不应该通过代码的方式切换offboard和解锁，这样会很危险），所以如果进行后续的开发还请大家仔细读懂其中的每一行代码，优化成一套有安全保证的精简的代码。

贺兰石

博主你好，点击“Takeoff”命令，滑动确认窗口后Gazebo中的小飞机就飞不起来，QGC中报错：QGroundControl Daily: MAV_CMD_NAV_TAKEOFF command temporarily rejected，终端里报错：FCU: Takeoff denied! Please disarm and retry。请问这该如何解决呢？谢谢！

kimkk

请问您解决了这个问题吗？我也遇到了，莫名其妙的没法解锁并且起飞。

zxgcz888

请问新建的节点应该怎样运行呢？

cnlcncn

博主你好，最后一步仿真的时候需要usb外接飞控，我装了好几遍了，前面都没问题，每一个单独运行都没问题，结果一起就不行

daza110

代码运行之后   无人机没有反应是什么情况呢

danniker

请问解决了吗
[发呆]