diff --git a/.vscode/launch.json b/.vscode/launch.json index b71f597..3de728c 100644 --- a/.vscode/launch.json +++ b/.vscode/launch.json @@ -204,7 +204,9 @@ "-c", "${config:OUSTER_UDPCAP_PATH}", "-o", - "15208" + "15208", + "-p", + "20000" ], "osx": { "MIMode": "lldb", diff --git a/.vscode/settings.json b/.vscode/settings.json index ddb85e6..85bc301 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -21,6 +21,7 @@ "terminal.integrated.env.linux": { "GTK_PATH": "" }, - "OUSTER_META_PATH": "~/Downloads/os-1-128.json", + "OUSTER_META_PATH": "~/Documents/os-1-128.json", + "OUSTER_UDPCAP_PATH": "~/Documents/officebot.udpbin", "OUSTER_HOST": "192.168.1.137", } \ No newline at end of file diff --git a/tools/ouster_replay1/src/ouster_replay.c b/tools/ouster_replay1/src/ouster_replay.c index 75a2985..2de0893 100644 --- a/tools/ouster_replay1/src/ouster_replay.c +++ b/tools/ouster_replay1/src/ouster_replay.c @@ -1,5 +1,9 @@ +/* +https://github.com/csimmonds/periodic-threads/blob/master/timerfd.c +*/ + #include #include #include @@ -7,6 +11,8 @@ #include #include #include +#include +#include #include @@ -20,14 +26,57 @@ typedef struct FILE *read_file; ouster_meta_t meta; int offset; + int delay_us; } app_t; -static const char *const usages[] = { - "ouster_replay1 [options] [[--] args]", - "ouster_replay1 [options]", - NULL, +struct periodic_info { + int timer_fd; + unsigned long long wakeups_missed; }; +static int make_periodic(int period_us, struct periodic_info *info) +{ + int ret; + unsigned int ns; + unsigned int sec; + int fd; + struct itimerspec itval; + + /* Create the timer */ + fd = timerfd_create(CLOCK_MONOTONIC, 0); + info->wakeups_missed = 0; + info->timer_fd = fd; + if (fd == -1) + return fd; + + /* Make the timer periodic */ + sec = period_us / 1000000; + ns = (period_us - (sec * 1000000)) * 1000; + itval.it_interval.tv_sec = sec; + itval.it_interval.tv_nsec = ns; + itval.it_value.tv_sec = sec; + itval.it_value.tv_nsec = ns; + ret = timerfd_settime(fd, 0, &itval, NULL); + return ret; +} + +static void wait_period(struct periodic_info *info) +{ + unsigned long long missed; + int ret; + /* Wait for the next timer event. If we have missed any the + number is written to "missed" */ + ret = read(info->timer_fd, &missed, sizeof(missed)); + if (ret == -1) { + perror("read timer"); + return; + } + info->wakeups_missed += missed; +} + + + + int main(int argc, char const *argv[]) { printf("ouster_replay1================================\n"); @@ -38,38 +87,41 @@ int main(int argc, char const *argv[]) .ip_dst = "127.0.0.1", .offset = 0}; - struct argparse_option options[] = { - OPT_HELP(), - OPT_GROUP("Basic options"), - OPT_STRING('m', "metafile", &app.metafile, "The meta file that correspond to the LiDAR sensor configuration", NULL, 0, 0), - OPT_STRING('c', "capture", &app.read_filename, "The capture file to replay", NULL, 0, 0), - OPT_STRING('d', "destination", &app.ip_dst, "The destination ip address. (Optional, default=127.0.0.1)", NULL, 0, 0), - OPT_INTEGER('o', "offset", &app.offset, "Where to start replay", NULL, 0, 0), - OPT_END(), - }; - - struct argparse argparse; - argparse_init(&argparse, options, usages, 0); - argparse_describe(&argparse, "\nA brief description of what the program does and how it works.", "\nAdditional description of the program after the description of the arguments."); - argc = argparse_parse(&argparse, argc, argv); - - if (app.metafile == NULL) { - argparse_usage(&argparse); - return -1; - } - - if (app.ip_dst == NULL) { - argparse_usage(&argparse); - return -1; - } - - if (app.read_filename == NULL) { - argparse_usage(&argparse); - return -1; + { + static const char *const usages[] = { + "ouster_replay1 [options] [[--] args]", + "ouster_replay1 [options]", + NULL, + }; + struct argparse_option options[] = { + OPT_HELP(), + OPT_GROUP("Basic options"), + OPT_STRING('m', "metafile", &app.metafile, "The meta file that correspond to the LiDAR sensor configuration", NULL, 0, 0), + OPT_STRING('c', "capture", &app.read_filename, "The capture file to replay", NULL, 0, 0), + OPT_STRING('d', "destination", &app.ip_dst, "The destination ip address. (Optional, default=127.0.0.1)", NULL, 0, 0), + OPT_INTEGER('o', "offset", &app.offset, "Where to start replay", NULL, 0, 0), + OPT_INTEGER('p', "period", &app.delay_us, "period us per frame", NULL, 0, 0), + OPT_END(), + }; + struct argparse argparse; + argparse_init(&argparse, options, usages, 0); + argparse_describe(&argparse, "\nA brief description of what the program does and how it works.", "\nAdditional description of the program after the description of the arguments."); + argc = argparse_parse(&argparse, argc, argv); + if (app.metafile == NULL) { + argparse_usage(&argparse); + return -1; + } + if (app.ip_dst == NULL) { + argparse_usage(&argparse); + return -1; + } + if (app.read_filename == NULL) { + argparse_usage(&argparse); + return -1; + } } ouster_log("Opening file '%s'\n", app.read_filename); - app.read_file = fopen(app.read_filename, "r"); if (app.read_file == NULL) { char buf[1024]; @@ -88,10 +140,9 @@ int main(int argc, char const *argv[]) } ouster_meta_parse(content, &app.meta); free(content); + printf("Column window: %i %i\n", app.meta.mid0, app.meta.mid1); } - printf("Column window: %i %i\n", app.meta.mid0, app.meta.mid1); - ouster_net_sock_desc_t desc = { .flags = OUSTER_NET_FLAGS_UDP}; int sock = ouster_net_create(&desc); @@ -112,6 +163,9 @@ int main(int argc, char const *argv[]) packet_id++; } + struct periodic_info info; + make_periodic(app.delay_us, &info); + while (1) { cap->size = OUSTER_NET_UDP_MAX_SIZE; @@ -130,7 +184,8 @@ int main(int argc, char const *argv[]) packet_id++; printf("%ju : ip=%s:%ji, sent=%ji of %ji\n", (uintmax_t)packet_id, app.ip_dst, (intmax_t)cap->port, (intmax_t)nsend, (intmax_t)cap->size); // getchar(); - nanosleep((const struct timespec[]){{0, 10000000L}}, NULL); + //nanosleep((const struct timespec[]){{0, 10000000L}}, NULL); + wait_period(&info); } return 0;