Qt Quick与ROS通过UDP协议实现网络通信

实现目标

项目需要编写一个无人机地面站，无人机在ROS系统下运行，地面站需要与无人机建立通信，能够控制无人机起飞、降落、飞行，并能够接收无人机的状态信息。
该无人机系统的组成如下图所示：

地面站通过无线网络与无人机上位机建立通信，上位机负责将飞控的无人机状态数据转发给地面站，并接收地面站的指令，再根据指令控制无人机执行相应的任务。该文章的目标就是构建图中网络通信的部分，首先以控制无人机起飞为例。

项目通过两部分实现：
1、在Qt Quick中重写QUdpSocket类，实现地面站发送UDP消息
2、在创建一个ROS节点，接收地面站的消息并控制无人机起飞

预备知识

1、了解UDP协议

无线通信网络通过UDP协议构建，UDP（用户数据报协议）是一种无连接的网络协议，UDP的主要特点包括无连接性、不保证可靠交付和面向报文的传输方式。由于UDP不建立连接，因此减少了开销和延迟，但同时也意味着它不保证数据的可靠传输。UDP适用于那些对实时性要求高，但可以容忍一定程度数据丢失的应用。
通俗的说，UDP通信类似于ROS中的话题，发送者只管往网络中不停发送消息，并不考虑是否有人接收以及接收者是否来得及接收；而接收者则是按照指定的频率不停接收网络中的消息。（与此相类比的，TCP协议则类似于ROS中的服务，这里暂不多提，后续若有用到再记录）

2、Qt Quick的使用

QML（Qt Meta-Object Language）是一种用于创建用户界面的声明性语言。它使用JSON格式的语法来描述用户界面，可以快速地创建出具有良好交互性和动画效果的应用程序。Qt Quick是一种用于编写应用程序的框架，在该框架中，开发者通过QML编写前端程序，通过C++编写后端程序。
在使用DUP协议搭建无线通信网络前，需要先掌握QML与C++的交互，这篇博客中有很详细的教程https://blog.csdn.net/xiezhongyuan07/article/details/109245920, 该项目中用到了“将C++类名注册到QML，并在QML声明一个对象并进行访问“的方法实现QML与C++的交互。

3、掌握ROS系统的使用

该项目的ROS系统在概念上分为了3部分，第一部分负责无人机仿真，第二部分运行飞控中的程序，第三部分负责运行上位机程序。该项目的核心在于搭建网络通信，这部分代码写在上位机程序中，因此只需掌握ROS的基本使用，就能完成无线网络的搭建；但若要完成的实现该项目，还需了解PX4无人机及其offboard模式，详细内容可在PX4官网(https://docs.px4.io/main/zh/ros/mavros_offboard_cpp.html)或该博客(https://blog.csdn.net/weixin_45031928/article/details/135142557)学习。

在Qt Quick中发送UDP消息到网络

首先在.pro文件中的头部添加Qt += network，指定当前项目使用网络模块。

随后在左侧项目栏右键添加新的文件，按如下截图创建新的C++文件：



创建完C++文件后我们需要引入QUdpSocket类，并在此基础上定义我们自己的类，来完成发送UDP消息的任务。


下面贴上代码和各部分的注释

#ifndef MYUDPSOCKET_H
#define MYUDPSOCKET_H

#include <QObject>
#include <QUdpSocket>  //引入QUdpSocket库

//重写定义自己的MyUdpSocket类，继承自QUdpSocket
class MyUdpSocket : public QUdpSocket
{
    Q_OBJECT  //编译器会编译带有Q_OBJECT宏的C++类，且该宏声明了一些成员函数，以实现信号和槽的功能
public:
    explicit MyUdpSocket(QObject *parent = nullptr);

    Q_INVOKABLE void sendMessage();  //Q_INVOKABLE标识可以将该函数暴露给QML语言，在QML编程时可以直接使用该函数

signals:  //定义一些信号，在调用该类的时候会发送这些信号，例如我们常用的onClicked信号和onCompeled信号
};

#endif // MYUDPSOCKET_H

#include "MyUdpSocket.h"
#include <QNetworkDatagram>  //该库中的函数用来发送UDP消息
#include <QHostAddress>      //该库用来获取主机ip地址

//在构造函数中绑定本机的ip地址，并设置端口号为65535
MyUdpSocket::MyUdpSocket(QObject *parent)
    : QUdpSocket{parent}
{
    bind(QHostAddress::LocalHost, 65535);
}

//sendMessage函数用于发送UDP消息
void MyUdpSocket::sendMessage()
{
    QByteArray data = "start";
    int bytesWritten = writeDatagram(data, 8, QHostAddress::LocalHost, 65535);  //writeDatagram用于发送UDP消息，4个参数分别是：发送的数据、字节数、ip地址、端口号（发送失败返回-1）

    if(bytesWritten != -1){
        qDebug()<<"发送成功"<<bytesWritten<<"字节";
    }else{
        qDebug()<<"发送失败";
    }
}

最后在main.cpp中包含MyUdpSocket.h文件，并添加上qmlRegisterType<MyUdpSocket>("myudpsocket", 1, 0, "MyUdpSocket");，便能够在QML文件中定义MyUdpSocket类的对象了。

下图展示了在QML文件中定义MyUdpSocket类的对象，并在在按下按钮时发送起飞消息的程序

Qt Quick部分的内容就此完成

在ROS节点中接收UDP消息

在offboard模式下控制PX4无人机起飞的代码中添加上接收UDP消息的部分（星号注释之间的为添加的内容），完整代码和注释如下：

/**
 * @file offb_node.cpp
 * @brief Offboard control example node, written with MAVROS version 0.19.x, PX4 Pro Flight
 * Stack and tested in Gazebo Classic 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>

#include <stdio.h>    //包含所需的库函数
#include <unistd.h>
#include <stdlib.h>
#include <sys/socket.h>
#include <sys/stat.h>
#include <string.h>
#include <arpa/inet.h>

mavros_msgs::State current_state;
void state_cb(const mavros_msgs::State::ConstPtr& msg){
    current_state = *msg;
}

geometry_msgs::PoseStamped current_pos;
void pos_cb(const geometry_msgs::PoseStamped::ConstPtr& msg){
    current_pos = *msg;
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "offb_node");
    ros::NodeHandle nh;

    /*******************************************************************************/
    int fd = socket(AF_INET, SOCK_DGRAM, 0);  //创建socket嵌套字（socket类似于单片机里的收发缓存区？大概），AF_INET是IPV4协议簇，SOCK_DGRAM指UDP数据报
    if(fd == -1){
      perror("socket error");
      exit(1);
    }  

    struct sockaddr_in serv;  //创建sockaddr_in类型的服务器结构体serv，该结构体包含四个成员：地址类型sin_family、端口号sin_port、IP地址sin_addr、填充字节sin_zero
    memset(&serv, 0, sizeof(serv));  //将结构体中的数据清零
    serv.sin_family = AF_INET;
    serv.sin_port = htons(65535);
    serv.sin_addr.s_addr = htonl(INADDR_ANY);  //INADDR_ANY参数表示绑定到所有可用的IP地址上

    int ret = bind(fd, (struct sockaddr*)&serv, sizeof(serv));  //绑定结构体，传入参数分别是：socket返回值、结构体地址、结构体长度
    if(ret == -1){
        perror("bind error");
        exit(1);
    }

    struct sockaddr_in client;  //创建客户端结构体
    socklen_t cli_len = sizeof(client);

    char buf[1024] = {0};  //创建接收数据缓存数组

    int start = 0;
    /*******************************************************************************/

    ros::Subscriber state_sub = nh.subscribe<mavros_msgs::State>
            ("mavros/state", 10, state_cb);
    ros::Subscriber local_pos = nh.subscribe<geometry_msgs::PoseStamped>
            ("mavros/local_position/pose", 10, pos_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 target_pose;
    target_pose.pose.position.x = 0;
    target_pose.pose.position.y = 0;
    target_pose.pose.position.z = 2;

    //send a few setpoints before starting
    for(int i = 100; ros::ok() && i > 0; --i){
        local_pos_pub.publish(target_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(start == 0){
            int recvlen = recvfrom(fd, buf, sizeof(buf), 0,   //当没有接收到数据时，程序在此阻塞；当接收到数据时，将客户端接收到的数据存入buf中，并将start置1，开始运行后续的起飞程序
                        (struct sockaddr*)&client, &cli_len);
            if(recvlen == -1){
                perror("recvform error");
                exit(1);
            }
            printf("received data: %s\n", buf);
            start = 1;
        }
        /*******************************************************************************/
        

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

        ros::spinOnce();
        rate.sleep();

    }

    return 0;
}

下面的部分具体展示了sockaddr_in类型结构体的成员

    struct sockaddr_in{
     unsigned short         sin_family;    
     unsigned short int     sin_port;      
     struct in_addr         sin_addr;      
     unsigned char          sin_zero[8];   
  };

ROS部分的程序就此修改完成

运行

按照顺序运行PX4仿真程序，打开QGC地面站，再启动刚刚编写的ROS节点，运行Qt程序，点击起飞按钮，可以看见无人机开始缓缓起飞。