PX4+Gazebo仿真：通过键盘控制无人机飞行

0. 飞行效果

飞行效果

1.前期准备

实验环境：Ubuntu20.04，ROS noetic

1.1 完成PX4的安装和编译

1.2 完成mavros 的安装

1.3 完成QGC的安装

上述内容网上资料很多，这里就不赘述了。

2.代码思路

代码思路参考PX4官网的Offboard control 的思路，重点在于如何将键盘上的内容转化为无人机的飞行信息。

代码1：无人机节点控制

#include <ros/ros.h>
#include <iostream>
#include <eigen3/Eigen/Dense>
#include <geometry_msgs/PoseStamped.h>
#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/State.h>
#include <std_msgs/String.h>

using namespace Eigen;
using namespace std;
//定义话题名称
ros::Publisher local_pos_pub;
ros::Subscriber key_sub;
// 计算无人机与目标位置的距离
Eigen::Vector3d target_position;
bool position_updated  = false;

void callback(const std_msgs::String::ConstPtr &msg)
{
    ROS_INFO("Received keyboard input: %s", msg->data.c_str());
    position_updated  = true;
    // 根据接收到的字符执行不同操作
    //w,a,s,d控制前后左右，p,l控制高度
    if (msg->data == "w")
    {
        target_position = target_position + Eigen::Vector3d(1.0, 0, 0);
    }
    else if (msg->data == "s")
    {
        target_position = target_position + Eigen::Vector3d(-1, 0, 0);
    }
    else if (msg->data == "a")
    {
        target_position = target_position + Eigen::Vector3d(0, 1, 0);
    }
    else if (msg->data == "d")
    {
        target_position = target_position + Eigen::Vector3d(0, -1, 0);
    }
    else if (msg->data == "p")
    {
        target_position = target_position + Eigen::Vector3d(0, 0, 1);
    }
    else if (msg->data == "l")
    {
        target_position = target_position + Eigen::Vector3d(0, 0, -1);
    }
    else
    {
        ROS_INFO("input must be 'w,a,s,d,p,l'");
        position_updated  = false;
    }
    
}

int main(int argc, char **argv)
{

    ros::init(argc, argv, "offb_node");
    ros::NodeHandle nh;
    ros::Subscriber key_sub = nh.subscribe("keyboard_input", 10, callback);
    ros::Publisher local_pos_pub = nh.advertise<geometry_msgs::PoseStamped>("mavros/setpoint_position/local", 10);
    // the setpoint publishing rate MUST be faster than 2Hz
    ros::Rate rate(20.0);

    // wait for FCU connection
    target_position << 0.0, 0.0, 1.0;
    while (ros::ok())
    {
        geometry_msgs::PoseStamped pose;
        pose.pose.position.x = target_position[0];
        pose.pose.position.y = target_position[1];
        pose.pose.position.z = target_position[2];
        if ( position_updated )
        {
            ROS_INFO("Target position %f,%f,%f", target_position[0], target_position[1], target_position[2]);
            position_updated = false;
        }

        local_pos_pub.publish(pose);
        ros::spinOnce();
        rate.sleep();
    }

    return 0;
}

本代码比较简单，两个话题分别对应键盘的输入和无人机位置的输出。具体内容可参考代码里的注释，这里不赘述。

值得注意的一点是，这里并没有像官网代码那里一样在代码里切换Offboard模式和Arm，因为在实际实验中，这种代码是非常危险的，容易导致无人机无法切换到其他模式，因此这里使用地面站QGC进行切换飞行模式

代码2： 键盘输入代码

#include <ros/ros.h>
#include <std_msgs/String.h>
#include <termios.h>
#include <unistd.h>
#include <fcntl.h>

class KeyboardInput
{
public:
    KeyboardInput()
    {
        pub_ = nh_.advertise<std_msgs::String>("keyboard_input", 10);
        tcgetattr(STDIN_FILENO, &initial_settings_);
        termios new_settings = initial_settings_;
        new_settings.c_lflag &= ~(ICANON | ECHO);
        tcsetattr(STDIN_FILENO, TCSANOW, &new_settings);
        fcntl(STDIN_FILENO, F_SETFL, O_NONBLOCK);
    }

    ~KeyboardInput()
    {
        tcsetattr(STDIN_FILENO, TCSANOW, &initial_settings_);
    }

    void run()
    {
        ros::Rate rate(10);
        while (ros::ok())
        {
            char c;
            if (read(STDIN_FILENO, &c, 1) == 1)
            {
                std_msgs::String msg;
                msg.data = std::string(1, c);
                pub_.publish(msg);
            }
            ros::spinOnce();
            rate.sleep();
        }
    }

private:
    ros::NodeHandle nh_;
    ros::Publisher pub_;
    termios initial_settings_;
};

int main(int argc, char **argv)
{
    ros::init(argc, argv, "keyboard_input");
    KeyboardInput keyboard_input;
    keyboard_input.run();
    return 0;
}

这里直接让GPT写的代码，内容很好理解，只有一个发布话题将键盘的内容转为ROS信息发布出去即可。 

3.实验流程

3.1 进行PX4编译

打开终端，根据自己路径和PX4版本执行：

make px4_sitl gazebo

操作结果为gazebo中出现无人机模型

 

3.2 打开Mavros进行通讯

另外打开终端，执行

roslaunch mavros px4.launch fcu_url:="udp://:14540@127.0.0.1:14557"

可以通过指令

rostopic list

 验证通信是否成果，成果的话可以看到很多和无人机有关的话题。

3.3 打卡QGC，连接无人机

这里没啥说的，一般都是直接就连上了

不过大家可以在这个时候试着切换Offboard模式，这个时候是无法切换到offboard模式的，因为px4对切换至offboard模式做出了限制，即必须有控制执行发送一段时间之后才可以切换到offboard模式。

3.4运行offboard程序

打开终端，根据自己编译的位置执行：

rosrun offboard offboard_node 

 此时无人机应该没有任何反应，因为这个时候还未处于Offboard模式

3.5 通过QGC切换飞行模式并解锁

此时可以看到无人机首先飞到了（0，0，1）的位置

3.5执行键盘输入指令

根据自己编译的位置，执行

rosrun keyboard_control keyboard_input

之后在终端里输入wasdpl即可控制无人机 

无人机按照指令飞到其位置

4.后续计划

这里采用的是直接计算无人机的位置点来完成其位置的改变，后续计划通过利用速度、加速度来控制无人机，来达到更加真实的效果。

——2024.08.01