diff --git a/wireless_comm_pkg/src/tcpudp_node.cpp b/wireless_comm_pkg/src/tcpudp_node.cpp new file mode 100644 index 0000000..0f42a81 --- /dev/null +++ b/wireless_comm_pkg/src/tcpudp_node.cpp @@ -0,0 +1,217 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#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 <("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; +} + +