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
|
// Encoded in UTF-8
#include "udp2lcm.h"
#include <sys/signal.h>
pthread_t udpRecv, udpSend;
int8_t heartBeat[9] = {0};
pthread_mutex_t heartBeatMutex;
struct sockaddr_in clientAddr;
char clientIP[20] = "";
lcm_t *lcm;
pid_t httpServerPid = -1;
void sigIntHandler(int sig) {
if (httpServerPid != -1) {
kill(httpServerPid, SIGINT);
}
printf("Exiting...\n");
exit(sig);
}
int main() {
// 当遇到Ctrl-C时，递归地杀死所有子进程
signal(SIGINT, sigIntHandler);
if (!LCMInit()) {
fprintf(stderr, "Error: Failed to initialize LCM\n");
return -1;
}
// 初始化线程锁
pthread_mutex_init(&heartBeatMutex, NULL);
// 开启udp接收线程
pthread_create(&udpRecv, NULL, udpRecvHandler, NULL);
if ((httpServerPid = fork()) == 0) {
// 拉起服务器进程，手机端需要申请的文件/data/test/defaultMap.txt.txt
// ATTENTION: 这里的路径需要根据实际存储地图的路径来修改
chdir("/data/test");
/*
* execlp使用系统调用exec()执行一个程序
* 第一个参数是文件名，将在PATH中查找，最后一个参数必须是NULL
* 其他的是传递过去的参数，按一般约定，第一个参数是程序名，不被使用
* 因而这里出现了两个python
*/
execlp("python", "python", "-m", "http.server", NULL);
}
while (true) {
pause();
}
return 0;
}
bool LCMInit() {
// 多播地址范围：224.0.0.0~239.255.255.255
if ((lcm = lcm_create(NULL)) == NULL) {
fprintf(stderr, "Error: Failed to create Path Ctrl LCM\n");
return false;
}
return true;
}
void parseCmd(const char *buffer, int bytesReceived) {
if (buffer == NULL) {
fprintf(stderr, "Error: Invalid message\n");
return;
}
path_ctrl_t path;
robot_control_t robotCtrlData;
pose_t curPos;
if (buffer[0] == 0) {
// 手机端用来与udp2lcm服务器建立连接的初始化消息
// 也是手机端的心跳
;
} else if (buffer[0] == 1) {
// 建图过程中，手机端遥控小车进行移动，命令下发至轮控模块
path.cmd = buffer[1];
path.speed = buffer[2];
path_ctrl_t_publish(lcm, "wheel_ctrl", &path);
} else if (buffer[0] == 2) {
/*
* 结束建图，主要有以下几个任务：
* - 向轮控模块发送停止命令
* - 通知算法模块结束建图
* - 拉起http服务器进程（改由main拉起，省的每次拉起浪费资源）
* - 向手机端通知建图完成
* 开始通过lcm接受算法给出的实时坐标，存储至heartBeat数组中（对应格式）
*/
path.cmd = 0;
path.speed = 0;
path_ctrl_t_publish(lcm, "wheel_ctrl", &path);
printf("Save Map!\n");
robotCtrlInit(&robotCtrlData, 0, 32, 0, 0, 0, 0, 0);
robot_control_t_publish(lcm, "ROBOT_CONTROL", &robotCtrlData);
pose_t_subscribe(lcm, "CURRENTPOSE", poseHandler, NULL);
freeRobotCtrl(&robotCtrlData);
sleep(2);
robotCtrlInit(&robotCtrlData, 0, 10, 0, 7, 0, 0, 0);
pthread_mutex_lock(&heartBeatMutex);
robotCtrlData.dparams[4] = (double)(heartBeat[3] << 8 + heartBeat[4]);
robotCtrlData.dparams[5] = (double)(heartBeat[5] << 8 + heartBeat[6]);
robotCtrlData.dparams[6] = (double)(heartBeat[7] << 8 + heartBeat[8]);
pthread_mutex_unlock(&heartBeatMutex);
robot_control_t_publish(lcm, "ROBOT_CONTROL", &robotCtrlData);
freeRobotCtrl(&robotCtrlData);
} else if (buffer[0] == 3) {
// 手机发来的终点坐标，转发给算法模块
int x, y, sita;
x = buffer[3] << 8 | buffer[4];
y = buffer[5] << 8 | buffer[6];
sita = buffer[7] << 8 | buffer[8];
robotCtrlInit(&robotCtrlData, 0, 20, 0, 3, 0, 0, 0);
robotCtrlData.dparams[0] = x;
robotCtrlData.dparams[1] = y;
robotCtrlData.dparams[2] = sita;
robot_control_t_publish(lcm, "ROBOT_CONTROL", &robotCtrlData);
freeRobotCtrl(&robotCtrlData);
}
}
void poseHandler(const lcm_recv_buf_t *rbuf, const char *channel,
const pose_t *msg, void *userdata) {
int16_t x, y, sita;
x = (int16_t)msg->pos[0];
y = (int16_t)msg->pos[1];
sita = (int16_t)msg->pos[2];
while (sita > 180)
sita -= 360;
while (sita < -180)
sita += 360;
pthread_mutex_lock(&heartBeatMutex);
heartBeat[0] = 0x03;
heartBeat[1] = heartBeat[2] = 0;
heartBeat[3] = x >> 8;
heartBeat[4] = x;
heartBeat[5] = y >> 8;
heartBeat[6] = y;
heartBeat[7] = sita >> 8;
heartBeat[8] = sita;
pthread_mutex_unlock(&heartBeatMutex);
}
|
