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
|
#include <dirent.h>
#include <errno.h>
#include <fcntl.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <termios.h>
#include <unistd.h>
#define LIDAR_SERIAL "dce74d177fdc724ba5757727edc269bf"
#define WHEEL_SERIAL "0001"
enum usbDevice { lidar, wheel, other };
char device_path[256];
int identify_device(const char *port) {
char cmd[256];
snprintf(cmd, sizeof(cmd), "udevadm info --name=%s | grep 'E: ID_SERIAL='",
port);
FILE *fp = popen(cmd, "r");
if (!fp) {
perror("popen");
return other;
}
static char serial[256];
if (fgets(serial, sizeof(serial), fp) != NULL) {
if (strstr(serial, WHEEL_SERIAL)) {
pclose(fp);
return wheel;
} else if (strstr(serial, LIDAR_SERIAL)) {
pclose(fp);
return lidar;
}
}
pclose(fp);
return other;
}
const char *findUSBDev(const char *device_type) {
DIR *dir;
struct dirent *entry;
dir = opendir("/dev");
if (dir == NULL) {
perror("opendir");
return NULL;
}
while ((entry = readdir(dir)) != NULL) {
if (strncmp(entry->d_name, "ttyUSB", 6) == 0) {
snprintf(device_path, sizeof(device_path), "/dev/%s",
entry->d_name);
int dev_type = identify_device(device_path);
if ((dev_type == lidar && strcmp(device_type, "lidar") == 0) ||
(dev_type == wheel && strcmp(device_type, "wheel") == 0)) {
closedir(dir);
return device_path;
}
}
}
closedir(dir);
return NULL;
}
|
