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()<<"发送失败";}
}

在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_zeromemset(&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 2Hzros::Rate rate(20.0);// wait for FCU connectionwhile(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 startingfor(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();// if(current_state.armed && current_state.mode == "OFFBOARD"){//     if(target_pose.pose.position.z - current_pos.pose.position.z < 0.2){//         target_pose.pose.position.x = 3;//         target_pose.pose.position.y = 4;//         target_pose.pose.position.z = 5;//     }// }}return 0;
}