|
|
|
|
@ -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;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
