From 5a94f1758b1ab28c861e1408124ab39ebae8eada Mon Sep 17 00:00:00 2001 From: likun17 Date: Wed, 6 Dec 2023 20:50:19 +0800 Subject: [PATCH 1/2] uorb: add generator tool. e.g.: 1. uorb_generator -f /data/sensor/19700101000003/sensor_accel_uncal0.csv -t sensor_accel_uncal2 2. uorb_generator -s -r 5 -n 200 -t sensor_accel_uncal2 timestamp:1,x:-2.0,y:3,z:4,temperature:5 Signed-off-by: likun17 --- system/uorb/CMakeLists.txt | 16 ++ system/uorb/Kconfig | 4 + system/uorb/Makefile | 5 + system/uorb/generator.c | 391 +++++++++++++++++++++++++++++++++++++ 4 files changed, 416 insertions(+) create mode 100644 system/uorb/generator.c diff --git a/system/uorb/CMakeLists.txt b/system/uorb/CMakeLists.txt index 030339a5141..1d7c29168cb 100644 --- a/system/uorb/CMakeLists.txt +++ b/system/uorb/CMakeLists.txt @@ -51,6 +51,22 @@ if(CONFIG_UORB) uorb) endif() + if(CONFIG_UORB_GENERATOR) + nuttx_add_application( + NAME + uorb_generator + PRIORITY + ${CONFIG_UORB_PRIORITY} + STACKSIZE + ${CONFIG_UORB_STACKSIZE} + MODULE + ${CONFIG_UORB} + SRCS + generator.c + DEPENDS + uorb) + endif() + if(CONFIG_UORB_TEST) nuttx_add_application( NAME diff --git a/system/uorb/Kconfig b/system/uorb/Kconfig index bbe66ca16a8..4e8103d2e4e 100644 --- a/system/uorb/Kconfig +++ b/system/uorb/Kconfig @@ -22,6 +22,10 @@ config UORB_LISTENER bool "uorb listener" default n +config UORB_GENERATOR + bool "uorb generator" + default n + config UORB_TESTS bool "uorb unit tests" default n diff --git a/system/uorb/Makefile b/system/uorb/Makefile index 2811f2f78e3..1bd27839207 100644 --- a/system/uorb/Makefile +++ b/system/uorb/Makefile @@ -36,6 +36,11 @@ MAINSRC += listener.c PROGNAME += uorb_listener endif +ifneq ($(CONFIG_UORB_GENERATOR),) +MAINSRC += generator.c +PROGNAME += uorb_generator +endif + ifneq ($(CONFIG_UORB_TESTS),) CSRCS += test/utility.c MAINSRC += test/unit_test.c diff --git a/system/uorb/generator.c b/system/uorb/generator.c new file mode 100644 index 00000000000..5740653349a --- /dev/null +++ b/system/uorb/generator.c @@ -0,0 +1,391 @@ +/**************************************************************************** + * apps/system/uorb/generator.c + * + * Licensed to the Apache Software Foundation (ASF) under one or more + * contributor license agreements. See the NOTICE file distributed with + * this work for additional information regarding copyright ownership. The + * ASF licenses this file to you under the Apache License, Version 2.0 (the + * "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include +#include + +#include +#include + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +#define GENERATOR_CACHE_BUFF 4096 + +/**************************************************************************** + * Private Types + ****************************************************************************/ + +struct sensor_gen_info_s +{ + struct orb_object obj; + FAR FILE *file; +}; + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +static bool g_should_exit = false; + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +static void usage(void) +{ + uorbinfo_raw("\n\ +The tool publishes topic data via uorb.\n\ +Notice:NSH_LINELEN must be set to 128 or more.\n\ +\n\ +generator [arguments...]\n\ + Commands:\n\ +\t The playback topic name.\n\ +\t[-h ] Listener commands help.\n\ +\t[-f ] File path to be played back(absolute path).\n\ +\t[-n ] Number of playbacks(fake model), default: 1\n\ +\t[-r ] The rate for playing fake data is only valid\ + when parameter 's' is used. default:10hz.\n\ +\t[-s ] Playback fake data.\n\ +\t[-t ] Playback topic.\n\ +\t e.g.:\n\ +\t\tsim - sensor_accel0:\n\ +\t\t uorb_generator -n 100 -r 5 -s -t sensor_accel0 timestamp:23191100,\ +x:0.1,y:9.7,z:0.81,temperature:22.15\n\n\ +\t\tsim - sensor_baro0:\n\ +\t\t uorb_generator -n 100 -r 5 -s -t sensor_baro0 timestamp:23191100,\ +pressure:999.12,temperature:26.34\n\n\ +\t\tfies - sensor_accel1\n\ +\t\t uorb_generator -f /data/uorb/20240823061723/sensor_accel0.csv\ + -t sensor_accel1\ +\t\t\n\ + "); +} + +static void exit_handler(int signo) +{ + g_should_exit = true; +} + +static int get_play_orb_id(FAR const char *filter, + FAR struct sensor_gen_info_s *info) +{ + struct orb_object object; + + if (filter) + { + object.meta = orb_get_meta(filter); + if (object.meta == NULL) + { + uorbinfo_raw("Input error built-in name:[%s]", filter); + return ERROR; + } + + object.instance = 0; + if (isdigit(filter[strlen(filter) - 1])) + { + object.instance = filter[strlen(filter) - 1] - '0'; + } + + info->obj = object; + return OK; + } + + uorbinfo_raw("The entered built-in topic name is empty."); + return ERROR; +} + +/**************************************************************************** + * Name: replay_worker + * + * Description: + * Playback data files. + * + * Input Parameters: + * sensor_gen Generator objects. + * + * Returned Value: + * 0 on success, otherwise -1 + ****************************************************************************/ + +static int replay_worker(FAR struct sensor_gen_info_s *sensor_gen) +{ + struct lib_meminstream_s meminstream; + bool is_frist = true; + uint64_t last_time; + uint64_t tmp_time; + FAR uint8_t *data; + FAR char *line; + int sleep_time; + int lastc; + int ret; + int fd; + + line = kmm_zalloc(GENERATOR_CACHE_BUFF); + if (line == NULL) + { + return -ENOMEM; + } + + if (fgets(line, GENERATOR_CACHE_BUFF, sensor_gen->file) != NULL) + { + if (strstr(line, sensor_gen->obj.meta->o_name) == NULL) + { + uorbinfo_raw("Topic and file do not match!"); + return -EINVAL; + } + } + else + { + uorbinfo_raw("Playback file format error!"); + return -EINVAL; + } + + data = kmm_zalloc(sensor_gen->obj.meta->o_size); + if (data == NULL) + { + return -ENOMEM; + } + + fd = orb_advertise_multi_queue_persist(sensor_gen->obj.meta, + data, &sensor_gen->obj.instance, 1); + if (fd < 0) + { + uorbinfo_raw("Playback orb advertise failed[%d]!", fd); + kmm_free(data); + return fd; + } + + while (fgets(line, GENERATOR_CACHE_BUFF, sensor_gen->file) != NULL) + { + lib_meminstream(&meminstream, line, GENERATOR_CACHE_BUFF); + ret = lib_bscanf(&meminstream.common, &lastc, + sensor_gen->obj.meta->o_format, data); + if (ret >= 0) + { + tmp_time = *(uint64_t *)data; + if (is_frist) + { + last_time = *(uint64_t *)data; + is_frist = false; + } + else + { + sleep_time = tmp_time - last_time; + if (sleep_time > 0) + { + nxsig_usleep(sleep_time); + } + + last_time = tmp_time; + } + + if (OK != orb_publish(sensor_gen->obj.meta, fd, data)) + { + uorbinfo_raw("Topic publish error!"); + break; + } + } + else + { + uorbinfo_raw("[ignore] The text data for this line is wrong![%s]!", + line); + } + } + + orb_unadvertise(fd); + kmm_free(data); + return 0; +} + +/**************************************************************************** + * Name: fake_worker + * + * Description: + * Publish fake data. + * + * Input Parameters: + * sensor_gen Generator objects. + * nb_cycle Number of publications. + * topic_rate Publish frequency. + * buffer String data to publish. + * + * Returned Value: + * 0 on success, otherwise -1 + ****************************************************************************/ + +static int fake_worker(FAR struct sensor_gen_info_s *sensor_gen, + int nb_cycle, float topic_rate, FAR char *buffer) +{ + struct lib_meminstream_s meminstream; + FAR uint8_t *data; + uint64_t interval; + int lastc; + int fd; + int i; + + if (buffer == NULL) + { + return -EINVAL; + } + + data = kmm_zalloc(sensor_gen->obj.meta->o_size); + if (data == NULL) + { + return -ENOMEM; + } + + lib_meminstream(&meminstream, buffer, strlen(buffer)); + if (lib_bscanf(&meminstream.common, &lastc, + sensor_gen->obj.meta->o_format, data) < 0) + { + uorbinfo_raw("Input string data parsing error![%s]", buffer); + goto error; + } + + fd = orb_advertise_multi_queue_persist(sensor_gen->obj.meta, + data, &sensor_gen->obj.instance, 1); + if (fd < 0) + { + uorbinfo_raw("Fake orb advertise failed[%d]!", fd); + goto error; + } + + interval = topic_rate ? (1000000 / topic_rate) : 500000; + + for (i = 0; i < nb_cycle; ++i) + { + *(uint64_t *)data = orb_absolute_time(); + if (OK != orb_publish(sensor_gen->obj.meta, fd, data)) + { + uorbinfo_raw("Topic publish error!"); + goto error; + } + + nxsig_usleep(interval); + } + + kmm_free(data); + return 0; + +error: + kmm_free(data); + return -1; +} + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +int main(int argc, FAR char *argv[]) +{ + FAR struct sensor_gen_info_s sensor_tmp; + float topic_rate = 0.0f; + FAR char *filter = NULL; + FAR char *topic = NULL; + FAR char *path = NULL; + int nb_cycle = 1; + bool sim = false; + int opt; + int ret; + + g_should_exit = false; + if (signal(SIGINT, exit_handler) == SIG_ERR) + { + return 1; + } + + while ((opt = getopt(argc, argv, "f:t:r:n:sh")) != -1) + { + switch (opt) + { + case 'f': + path = optarg; + break; + + case 't': + topic = optarg; + break; + + case 'r': + topic_rate = atof(optarg); + if (topic_rate < 0) + { + goto error; + } + break; + + case 'n': + nb_cycle = strtol(optarg, NULL, 0); + if (nb_cycle < 1) + { + goto error; + } + break; + + case 's': + sim = true; + break; + + case 'h': + default: + goto error; + } + } + + if (optind < argc) + { + filter = argv[optind]; + } + + ret = get_play_orb_id(topic, &sensor_tmp); + if (ret < 0) + { + return -ERROR; + } + + if (sim) + { + ret = fake_worker(&sensor_tmp, nb_cycle, topic_rate, filter); + } + else + { + sensor_tmp.file = fopen(path, "r"); + if (sensor_tmp.file == NULL) + { + uorbinfo_raw("Failed to open file:[%s]!", path); + return -ERROR; + } + + ret = replay_worker(&sensor_tmp); + fclose(sensor_tmp.file); + } + + return ret; + +error: + usage(); + return -ERROR; +} From d384b5d39fb1f0727bf23ad8252c74bf4a9c415a Mon Sep 17 00:00:00 2001 From: likun17 Date: Wed, 19 Nov 2025 17:38:55 +0800 Subject: [PATCH 2/2] uorb/generator:Fix generator bugs and formatting issues. replace kmm_zalloc with zalloc and malloc, and fix format and return value errors. Signed-off-by: likun17 --- system/uorb/generator.c | 159 +++++++++++++++++++++++----------------- 1 file changed, 93 insertions(+), 66 deletions(-) diff --git a/system/uorb/generator.c b/system/uorb/generator.c index 5740653349a..80ac28fa23c 100644 --- a/system/uorb/generator.c +++ b/system/uorb/generator.c @@ -48,7 +48,7 @@ struct sensor_gen_info_s * Private Data ****************************************************************************/ -static bool g_should_exit = false; +static bool g_gen_should_exit = false; /**************************************************************************** * Private Functions @@ -77,7 +77,7 @@ x:0.1,y:9.7,z:0.81,temperature:22.15\n\n\ \t\tsim - sensor_baro0:\n\ \t\t uorb_generator -n 100 -r 5 -s -t sensor_baro0 timestamp:23191100,\ pressure:999.12,temperature:26.34\n\n\ -\t\tfies - sensor_accel1\n\ +\t\tfiles - sensor_accel1\n\ \t\t uorb_generator -f /data/uorb/20240823061723/sensor_accel0.csv\ -t sensor_accel1\ \t\t\n\ @@ -86,27 +86,43 @@ pressure:999.12,temperature:26.34\n\n\ static void exit_handler(int signo) { - g_should_exit = true; + (void)signo; + g_gen_should_exit = true; } static int get_play_orb_id(FAR const char *filter, FAR struct sensor_gen_info_s *info) { struct orb_object object; + int len; + int idx; - if (filter) + if (filter == NULL) + { + uorbinfo_raw("The entered built-in topic name is NULL."); + return ERROR; + } + + len = strlen(filter); + if (len > 0) { object.meta = orb_get_meta(filter); - if (object.meta == NULL) + if (object.meta) { - uorbinfo_raw("Input error built-in name:[%s]", filter); - return ERROR; + object.instance = 0; + for (idx = 0; idx < len; idx++) + { + if (isdigit(filter[idx])) + { + object.instance = filter[idx] - '0'; + break; + } + } } - - object.instance = 0; - if (isdigit(filter[strlen(filter) - 1])) + else { - object.instance = filter[strlen(filter) - 1] - '0'; + uorbinfo_raw("The entered built-in topic name is invalid."); + return ERROR; } info->obj = object; @@ -133,7 +149,7 @@ static int get_play_orb_id(FAR const char *filter, static int replay_worker(FAR struct sensor_gen_info_s *sensor_gen) { struct lib_meminstream_s meminstream; - bool is_frist = true; + bool is_first = true; uint64_t last_time; uint64_t tmp_time; FAR uint8_t *data; @@ -143,7 +159,7 @@ static int replay_worker(FAR struct sensor_gen_info_s *sensor_gen) int ret; int fd; - line = kmm_zalloc(GENERATOR_CACHE_BUFF); + line = zalloc(GENERATOR_CACHE_BUFF); if (line == NULL) { return -ENOMEM; @@ -154,19 +170,22 @@ static int replay_worker(FAR struct sensor_gen_info_s *sensor_gen) if (strstr(line, sensor_gen->obj.meta->o_name) == NULL) { uorbinfo_raw("Topic and file do not match!"); - return -EINVAL; + ret = -EINVAL; + goto out_line; } } else { uorbinfo_raw("Playback file format error!"); - return -EINVAL; + ret = -EINVAL; + goto out_line; } - data = kmm_zalloc(sensor_gen->obj.meta->o_size); + data = zalloc(sensor_gen->obj.meta->o_size); if (data == NULL) { - return -ENOMEM; + ret = -ENOMEM; + goto out_line; } fd = orb_advertise_multi_queue_persist(sensor_gen->obj.meta, @@ -174,11 +193,12 @@ static int replay_worker(FAR struct sensor_gen_info_s *sensor_gen) if (fd < 0) { uorbinfo_raw("Playback orb advertise failed[%d]!", fd); - kmm_free(data); - return fd; + ret = fd; + goto out_data; } - while (fgets(line, GENERATOR_CACHE_BUFF, sensor_gen->file) != NULL) + while (fgets(line, GENERATOR_CACHE_BUFF, sensor_gen->file) != NULL && + !g_gen_should_exit) { lib_meminstream(&meminstream, line, GENERATOR_CACHE_BUFF); ret = lib_bscanf(&meminstream.common, &lastc, @@ -186,10 +206,10 @@ static int replay_worker(FAR struct sensor_gen_info_s *sensor_gen) if (ret >= 0) { tmp_time = *(uint64_t *)data; - if (is_frist) + if (is_first) { last_time = *(uint64_t *)data; - is_frist = false; + is_first = false; } else { @@ -205,6 +225,7 @@ static int replay_worker(FAR struct sensor_gen_info_s *sensor_gen) if (OK != orb_publish(sensor_gen->obj.meta, fd, data)) { uorbinfo_raw("Topic publish error!"); + ret = ERROR; break; } } @@ -216,8 +237,12 @@ static int replay_worker(FAR struct sensor_gen_info_s *sensor_gen) } orb_unadvertise(fd); - kmm_free(data); - return 0; + +out_data: + free(data); +out_line: + free(line); + return ret; } /**************************************************************************** @@ -251,7 +276,7 @@ static int fake_worker(FAR struct sensor_gen_info_s *sensor_gen, return -EINVAL; } - data = kmm_zalloc(sensor_gen->obj.meta->o_size); + data = malloc(sensor_gen->obj.meta->o_size); if (data == NULL) { return -ENOMEM; @@ -275,23 +300,25 @@ static int fake_worker(FAR struct sensor_gen_info_s *sensor_gen, interval = topic_rate ? (1000000 / topic_rate) : 500000; - for (i = 0; i < nb_cycle; ++i) + for (i = 0; i < nb_cycle && !g_gen_should_exit; ++i) { *(uint64_t *)data = orb_absolute_time(); if (OK != orb_publish(sensor_gen->obj.meta, fd, data)) { uorbinfo_raw("Topic publish error!"); - goto error; + goto error_fd; } nxsig_usleep(interval); } - kmm_free(data); + free(data); return 0; +error_fd: + orb_unadvertise(fd); error: - kmm_free(data); + free(data); return -1; } @@ -301,7 +328,7 @@ static int fake_worker(FAR struct sensor_gen_info_s *sensor_gen, int main(int argc, FAR char *argv[]) { - FAR struct sensor_gen_info_s sensor_tmp; + struct sensor_gen_info_s sensor_tmp; float topic_rate = 0.0f; FAR char *filter = NULL; FAR char *topic = NULL; @@ -311,7 +338,7 @@ int main(int argc, FAR char *argv[]) int opt; int ret; - g_should_exit = false; + g_gen_should_exit = false; if (signal(SIGINT, exit_handler) == SIG_ERR) { return 1; @@ -320,39 +347,39 @@ int main(int argc, FAR char *argv[]) while ((opt = getopt(argc, argv, "f:t:r:n:sh")) != -1) { switch (opt) - { - case 'f': - path = optarg; - break; - - case 't': - topic = optarg; - break; - - case 'r': - topic_rate = atof(optarg); - if (topic_rate < 0) - { - goto error; - } - break; - - case 'n': - nb_cycle = strtol(optarg, NULL, 0); - if (nb_cycle < 1) - { + { + case 'f': + path = optarg; + break; + + case 't': + topic = optarg; + break; + + case 'r': + topic_rate = atof(optarg); + if (topic_rate < 0) + { + goto error; + } + break; + + case 'n': + nb_cycle = strtol(optarg, NULL, 0); + if (nb_cycle < 1) + { + goto error; + } + break; + + case 's': + sim = true; + break; + + case 'h': + default: goto error; - } - break; - - case 's': - sim = true; - break; - - case 'h': - default: - goto error; - } + } } if (optind < argc) @@ -363,7 +390,7 @@ int main(int argc, FAR char *argv[]) ret = get_play_orb_id(topic, &sensor_tmp); if (ret < 0) { - return -ERROR; + return ERROR; } if (sim) @@ -376,7 +403,7 @@ int main(int argc, FAR char *argv[]) if (sensor_tmp.file == NULL) { uorbinfo_raw("Failed to open file:[%s]!", path); - return -ERROR; + return ERROR; } ret = replay_worker(&sensor_tmp); @@ -387,5 +414,5 @@ int main(int argc, FAR char *argv[]) error: usage(); - return -ERROR; + return ERROR; }