1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
|
#include <arpa/inet.h>
#include <chrono>
#include <cmath>
#include <condition_variable>
#include <cstring>
#include <iostream>
#include <mutex>
#include <netinet/in.h>
#include <sstream>
#include <string.h>
#include <string>
#include <sys/socket.h>
#include <thread>
#include <unistd.h>
using namespace std;
enum robotState { // 机器人状态枚举
beforeStart,
pending,
afterEnd, // 建好了图，接下来可选择重新建图或选终点开始路径
startRoute,
};
enum runState { stop, go, left, right };
struct ReceiveDataStruct {
int robotId;
robotState state;
int x;
int y;
int r;
};
const int PORT = 5001;
const char *SERVER_IP = "0.0.0.0";
const int MAX_BUFFER_SIZE = 1024;
struct ReceiveData {
int values[4];
};
bool newDataAvailable = false;
string CLIENT_IP = "127.0.0.1";
struct sockaddr_in clientAddr;
ReceiveData receivedData = {{0, 1, 2, 3}};
// UDP发送线程函数
void udpSenderThread() {
char values[100] = {0};
struct sockaddr_in serverAddr;
int sockfd = -1;
// 创建UDP套接字
if ((sockfd = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP)) == -1) {
fprintf(stderr, "Error: Failed to create socket\n");
return;
}
memset((char *)&serverAddr, 0, sizeof(serverAddr));
serverAddr.sin_family = AF_INET;
serverAddr.sin_port = htons(PORT);
if (inet_aton(CLIENT_IP.c_str(), &serverAddr.sin_addr) == 0) {
fprintf(stderr, "Error: Failed to convert IP address\n");
close(sockfd);
return;
}
while (true) {
this_thread::sleep_for(chrono::seconds(1)); // 每1秒发送一次
// 更新发送的消息
memset(values, 0, 100);
for (int i = 0; i < 4; ++i) {
// values[i] = receivedData.values[i] + 1;
sprintf(values + strlen(values), "%d ", receivedData.values[i] + 1);
}
sprintf(values + strlen(values), "\n");
printf("Sending message to %s: %s\n", CLIENT_IP.c_str(), values);
// 发送消息
sendto(sockfd, (const char *)values, sizeof(int) * 4, 0,
(struct sockaddr *)&serverAddr, sizeof(serverAddr));
}
close(sockfd);
}
// 主线程接收UDP消息并打印
void receiveAndPrintMessages() {
int sock;
struct sockaddr_in serverAddr;
socklen_t addrLen = sizeof(clientAddr);
char buffer[MAX_BUFFER_SIZE];
ReceiveData receivedData;
int pos, bytesReceived;
// 创建UDP套接字
if ((sock = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP)) == -1) {
fprintf(stderr, "Error: Failed to create socket\n");
return;
}
memset((char *)&serverAddr, 0, sizeof(serverAddr));
memset(buffer, 0, sizeof(buffer));
serverAddr.sin_family = AF_INET;
serverAddr.sin_port = htons(PORT);
serverAddr.sin_addr.s_addr = htonl(INADDR_ANY);
// 绑定到端口
if (::bind(sock, (struct sockaddr *)&serverAddr, sizeof(serverAddr)) ==
-1) {
cerr << "Error: Bind failed" << endl;
close(sock);
return;
}
bytesReceived = recvfrom(sock, buffer, MAX_BUFFER_SIZE, 0,
(struct sockaddr *)&clientAddr, &addrLen);
if (bytesReceived == -1) {
fprintf(stderr, "Error: Failed to receive data\n");
return;
}
printf("Received Message from %s: %s\n", inet_ntoa(clientAddr.sin_addr),
buffer);
CLIENT_IP = inet_ntoa(clientAddr.sin_addr);
thread senderThread2(udpSenderThread);
while (true) {
// 接收消息
bytesReceived = recvfrom(sock, buffer, MAX_BUFFER_SIZE, 0,
(struct sockaddr *)&clientAddr, &addrLen);
if (bytesReceived == -1) {
fprintf(stderr, "Error: Failed to receive data\n");
continue;
}
printf("Receive %d bytes from %s: \"%s\"\n", bytesReceived,
CLIENT_IP.c_str(), buffer);
pos = 0;
for (int i = 0; i < 4; ++i) {
receivedData.values[i] = atoi(strtok(buffer + pos, " "));
// pos += log10(receivedData.values[i]) + 2;
// pos需要递增，否则会一直解析第一个数，要求能处理数据为0或负数的情况
pos += strlen(strtok(buffer + pos, " ")) + 1;
}
newDataAvailable = true;
// 打印消息
// CLIENT_IP = inet_ntoa(clientAddr.sin_addr);
// cout << "Received Message from " << CLIENT_IP << ": ";
// printf("Received Message from %s: ", CLIENT_IP.c_str());
// for (int i = 0; i < 4; ++i) {
// printf("%d ", receivedData.values[i]);
// }
printf("\n");
}
close(sock);
senderThread2.join();
}
int main() {
thread receiverThread(receiveAndPrintMessages);
receiverThread.join();
return 0;
}
|
