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
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
|
// Encoded in UTF-8
/*
* struct path_ctrl_t
* {
* int8_t cmd;
* int8_t speed;
* }
* 这是与轮控模块通信的数据结构，speed为0-100表示速度
* cmd为0停，1前，2左，3右
*/
#include "path/path_ctrl_t.h"
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#define MAX_BUFFER_SIZE 1024
#define PORT 5001
pthread_t udpRecv, udpSend;
int8_t heartBeat[9] = {0}; // 用于存储需要发送的心跳信息
// clientIP用于存储手机端IP地址，仅在第一次接收消息后修改
// 本身就是inet_ntoa(clientAddr.sin_addr)，二者内容完全一致
struct sockaddr_in clientAddr;
char clientIP[20] = "";
lcm_t *path_ctrl_lcm;
bool LCMInit();
void udpSendHandler();
void udpRecvHandler();
void parseCmd(const char *buffer, int bytesReceived);
int main() {
if (!LCMInit()) {
fprintf(stderr, "Error: Failed to initialize LCM\n");
return -1;
}
// 开启udp接收线程
pthread_create(&udpRecv, NULL, udpRecvHandler, NULL);
return 0;
}
bool LCMInit() {
// TODO: 在这里同意初始化各个LCM变量
// ATTENTION:
// 是否在这里订阅各个通道、各个通道需要在什么地方订阅，需要慎重考虑
if ((path_ctrl_lcm = lcm_create(NULL)) == NULL) {
fprintf(stderr, "Error: Failed to create Path Ctrl LCM\n");
return false;
}
return true;
}
void udpRecvHandler() {
int socketfd = -1;
char buffer[MAX_BUFFER_SIZE];
struct sockaddr_in serverAddr;
socklen_t addrLen = sizeof(serverAddr);
int bytesReceived;
int retval;
struct pollfd fds;
if ((socketfd = 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(socketfd, (struct sockaddr *)&serverAddr, sizeof(serverAddr)) ==
-1) {
fprintf(stderr, "Error: Bind failed\n");
close(socketfd);
return;
}
bytesReceived = recvfrom(socketfd, buffer, MAX_BUFFER_SIZE, 0,
(struct sockaddr *)&clientAddr, &addrLen);
if (bytesReceived == -1) {
fprintf(stderr, "Error: Failed to receive data\n");
close(socketfd);
return;
}
printf("Start message from %s: %s\n", inet_ntoa(clientAddr.sin_addr),
buffer);
strcpy(clientIP, inet_ntoa(clientAddr.sin_addr));
// 在接收到第一条消息之后，开启udp发送线程，用于发送心跳信息
if (pthread_create(&udpSend, NULL, udpSendHandler, NULL) != 0) {
fprintf(stderr, "Error creating udpSend thread\n");
close(socketfd);
exit(-1);
}
fds.fd = socketfd;
fds.events = POLLIN;
while (true) {
retval = poll(&fds, 1, 3000);
if (retval == -1) {
fprintf(stderr, "Error: select failed\n");
break;
} else if (retval == 0) {
printf("No data within three seconds.\n");
speedControl(0, 0);
} else {
bytesReceived = recvfrom(socketfd, buffer, MAX_BUFFER_SIZE, 0,
(struct sockaddr *)&clientAddr, &addrLen);
if (bytesReceived == -1) {
fprintf(stderr, "Error: Failed to receive data\n");
continue;
}
printf("Massage from client %s: ", clientIP);
for (int i = 0; i < bytesReceived; i++) {
printf("%02x ", buffer[i]);
}
printf("\n");
parseCmd(buffer, bytesReceived);
}
}
}
void udpSendHandler() {
// 创建UDP套接字
int sockfd = -1;
struct sockaddr_in serverAddr;
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; // 设置地址族为IPv4
serverAddr.sin_port = htons(port); // 设置端口号
if (inet_aton(clientIP, &serverAddr.sin_addr) == 0) {
fprintf(stderr, "Error: Failed to convert IP address\n");
close(sockfd);
return;
}
while (true) {
sendto(sockfd, (const char *)heartBeat, 9, 0,
(struct sockaddr *)&serverAddr, sizeof(serverAddr));
printf("Send: Data %02x udpSend successfully!\n", a);
sleep(1);
}
}
void parseCmd(const char *buffer, int bytesReceived) {
// TODO: 完成本函数的内容
if (buffer == NULL || strlen(buffer) != 9) {
fprintf(stderr, "Error: Invalid message\n");
return;
}
if (buffer[0] == 0) {
// 手机端用来与udp2lcm服务器建立连接的初始化消息
fprintf(stderr, "Why Init cmd here?\n");
} else if (buffer[0] == 1) {
// 建图过程中，手机端遥控小车进行移动，命令下发至轮控模块
path_ctrl_t path;
path.cmd = buffer[1];
path.speed = buffer[2];
path_ctrl_t_publish(path_ctrl_lcm, "wheel_ctrl", &path);
} else if (buffer[0] == 2) {
/*
* 结束建图，主要有以下几个任务：
* - 向轮控模块发送停止命令
* - 通知算法模块结束建图（FIXME: 是否接受算法模块的回应？）
* - 拉起http服务器进程
* - 向手机端通知建图完成
* -
* 开始通过lcm接受算法给出的实时坐标，存储至heartBeat数组中（对应格式）
*/
} else if (buffer[0] == 3) {
// 手机发来的终点坐标，转发给算法模块
;
}
}
|
