添加漏提交的节点。Author：吴鸿

wireless_comm_pkg/src/tcpudp_node.cpp

@@ -0,0 +1,217 @@
+#include <iostream>
+#include <unistd.h>
+#include <cstring>
+#include <sys/types.h>
+#include <sys/socket.h>
+#include <netinet/in.h>
+#include <arpa/inet.h>
+#include <fcntl.h>
+#include <ros/ros.h>
+#include <signal.h>
+#include <netdb.h>
+#include <ns_msgs/Vms2NSCmd_TCP.h>
+
+#define PORT 12347   // 服务器监听的端口号
+#define TCP_REC_BUFFER_SIZE 3 // 规定的TCP接收报文字节长度，实际应为56，设为3作测试用
+#define TCP_SEND_BUFFER_SIZE 7 //TCP发送字节长度
+#define UDP_REC_BUFFER_SIZE 2    //UDP接收字节长度
+#define UDP_SEND_BUFFER_SIZE 31  //UDP发送字节长度
+int tcp_server_fd = -1; // 服务器套接字,此处fd指file descriptor，socket中的概念
+int tcp_client_fd = -1; // 客户端套接字,这列的客户端指VMS控制系统
+ssize_t total_received_len = 0;
+char tcp_buffer[TCP_REC_BUFFER_SIZE]; //接收缓存
+
+// CTRL+C强制退出的处理，保证释放端口占用
+void MySigintHandler(int sig)
+{
+	ROS_INFO("shutting down!");
+	ros::shutdown();
+    if (tcp_client_fd != -1) {
+        close(tcp_client_fd);
+        tcp_client_fd = -1;
+    }
+    if (tcp_server_fd != -1) {
+        close(tcp_server_fd);
+        tcp_server_fd = -1;
+    }
+    exit(0);
+}
+
+// 获取本机IP地址函数，暂时失败了，可忽略
+bool GetHostInfo(std::string& hostName, std::string& Ip) {
+	char name[256];
+	gethostname(name, sizeof(name));
+	hostName = name;
+	
+	struct hostent* host = gethostbyname(name);
+	char ipStr[32];
+	const char* ret = inet_ntop(host->h_addrtype, host->h_addr_list[0], ipStr, sizeof(ipStr));
+	if (NULL==ret) {
+		std::cout << "hostname transform to ip failed";
+		return false;
+	}
+	Ip = ipStr;
+	return true;
+}
+
+// 初始化tcp的函数
+int TcpInit(struct sockaddr_in &server_addr, struct sockaddr_in &client_addr,socklen_t &client_addr_len)
+{
+    // 创建套接字
+    tcp_server_fd = socket(AF_INET, SOCK_STREAM, 0);
+    if (tcp_server_fd == -1) {
+        std::cerr << "Failed to create socket" << std::endl;
+        return -1;
+    }
+    int opt = 1;
+    setsockopt(tcp_server_fd,SOL_SOCKET,SO_REUSEADDR,&opt,sizeof( opt ));    
+
+    // 设置服务器地址和端口
+    server_addr.sin_family = AF_INET;
+    server_addr.sin_addr.s_addr = INADDR_ANY;
+    server_addr.sin_port = htons(PORT);
+
+    // 绑定套接字到指定地址和端口
+    if (bind(tcp_server_fd, (struct sockaddr*)&server_addr, sizeof(server_addr)) == -1) {
+        std::cerr << "Failed to bind socket" << std::endl;
+        close(tcp_server_fd);
+        return -1;
+    }
+
+    int flags = fcntl(tcp_server_fd, F_GETFL, 0);
+    fcntl(tcp_server_fd, F_SETFL, flags | O_NONBLOCK);
+    
+    // 监听端口，等待连接请求
+    if (listen(tcp_server_fd, 5) == -1) {
+        std::cerr << "Failed to listen on socket" << std::endl;
+        close(tcp_server_fd);
+        return -1;
+    }
+
+    std::cout << "Server is listening on port " << PORT << std::endl;
+    return 1;
+}
+
+void TcpConnnect(struct sockaddr_in &server_addr, struct sockaddr_in &client_addr,socklen_t &client_addr_len, ros::Publisher &pub)
+{
+    // 如果没有和客户端（VMS）建立连接，就尝试建立连接
+    if (tcp_client_fd == -1)
+    {
+        // 接受客户端连接
+        tcp_client_fd = accept(tcp_server_fd, (struct sockaddr*)&client_addr, &client_addr_len);
+        if (tcp_client_fd == -1) 
+        {
+            if (errno == EWOULDBLOCK || errno == EAGAIN) 
+            {
+                // 没有新的连接请求，继续其他处理或休眠一段时间
+                usleep(100000); // 休眠100毫秒
+            } 
+            else 
+            {
+                std::cerr << "Failed to accept client connection" << std::endl;
+                ROS_ERROR("Failed to accept client connection");
+            }
+            // 建立连接失败一定会直接退出，不会进行后续处理
+            return;
+        }
+        // 设置客户端套接字为非阻塞模式
+        int flags = fcntl(tcp_client_fd, F_GETFL, 0);
+        fcntl(tcp_client_fd, F_SETFL, flags | O_NONBLOCK);
+
+        std::cout << "Client connected: " << inet_ntoa(client_addr.sin_addr) << std::endl;
+        total_received_len = 0;
+    }
+    // 此时 client_fd != -1;
+    // 尝试进行一次信息接收
+    ssize_t bytes_received = recv(tcp_client_fd, tcp_buffer + total_received_len, TCP_REC_BUFFER_SIZE - total_received_len, 0);
+    // 一般是没有新数据，如果有其他异常就中断连接
+    if (bytes_received == -1) 
+    {
+        if (errno == EWOULDBLOCK || errno == EAGAIN) 
+        {
+            // 没有新数据可读，继续其他处理或休眠一段时间
+            usleep(100000); // 休眠100毫秒
+        } 
+        else 
+        {
+            std::cerr << "Failed to receive data" << std::endl;
+            close(tcp_client_fd);
+            tcp_client_fd = -1;
+        }
+    } 
+    // 客户端断开TCP连接，释放客户端套接字
+    else if (bytes_received == 0) 
+    {
+        std::cout << "Client disconnected" << std::endl;
+        close(tcp_client_fd);
+        tcp_client_fd = -1;
+    } 
+    // 正常接收到数据，进行数据处理
+    else 
+    {
+        // 累计目前接收到的数据总量
+        total_received_len += bytes_received;
+        // 长度和报文长度一致，即开始解析
+        if (total_received_len == TCP_REC_BUFFER_SIZE) 
+        {
+            uint8_t sequence_number = tcp_buffer[0];
+            uint8_t value1 = tcp_buffer[1];
+            uint8_t value2 = tcp_buffer[2];
+
+            std::cout << "Received message:" << std::endl;
+            std::cout << "Sequence Number: " << (int)sequence_number << std::endl;
+            std::cout << "Value 1: " << (int)value1 << std::endl;
+            std::cout << "Value 2: " << (int)value2 << std::endl;
+
+            ns_msgs::Vms2NSCmd_TCP msg;
+            msg.sequence_id = sequence_number;
+            msg.command_type = value1;
+            msg.park_accuracy_type = value2;
+            pub.publish(msg);
+
+            std::string ack = "Complete Message received";
+            send(tcp_client_fd, ack.c_str(), ack.size(), 0);
+            total_received_len = 0;
+        }
+    }
+    
+}
+
+int main(int argc, char** argv)
+{
+    ros::init(argc, argv, "server_port");
+    signal(SIGINT, MySigintHandler);
+    std::string hostname, ip_addr;
+    if(GetHostInfo(hostname, ip_addr))
+    {
+        std::cout << hostname << "," << ip_addr <<std::endl;
+    }
+    // 创建ROS句柄
+    ros::NodeHandle nh;
+    ros::Publisher pub = nh.advertise<ns_msgs::Vms2NSCmd_TCP>("tcp_command_topic",10);
+    struct sockaddr_in server_addr, client_addr;
+    socklen_t client_addr_len = sizeof(client_addr);
+
+    int connect_result = TcpInit(server_addr, client_addr, client_addr_len);
+    if (connect_result != 1) {
+        ROS_ERROR("TCP init failed.");
+        return 0;
+    }
+
+    // 每秒循环次数，需考虑修改
+    ros::Rate loop_rate(10);
+
+    // 循环接收和打印数据
+    while (ros::ok()) {
+        // 传递引用，进行一次信息接收的尝试
+        TcpConnnect(server_addr, client_addr, client_addr_len, pub);
+        ros::spinOnce();
+    }
+
+    // 关闭套接字
+    close(tcp_client_fd);
+    close(tcp_server_fd);
+    return 0;
+}
+
+