This is an automated email from the ASF dual-hosted git repository. xiaoxiang pushed a commit to branch master in repository https://gitbox.apache.org/repos/asf/nuttx-apps.git
commit 9cf9b862c15744728ce3aeb815479ad491662280 Author: likun17 <[email protected]> AuthorDate: Wed Dec 6 20:50:19 2023 +0800 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 <[email protected]> --- system/uorb/CMakeLists.txt | 16 ++ system/uorb/Kconfig | 4 + system/uorb/Makefile | 5 + system/uorb/generator.c | 391 +++++++++++++++++++++++++++++++++++++++++++++ 4 files changed, 416 insertions(+) diff --git a/system/uorb/CMakeLists.txt b/system/uorb/CMakeLists.txt index 030339a51..1d7c29168 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 bbe66ca16..4e8103d2e 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 2811f2f78..1bd278392 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 000000000..574065334 --- /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 <nuttx/signal.h> +#include <nuttx/streams.h> + +#include <ctype.h> +#include <uORB/uORB.h> + +/**************************************************************************** + * 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 <command> [arguments...]\n\ + Commands:\n\ +\t<topics_name> The playback topic name.\n\ +\t[-h ] Listener commands help.\n\ +\t[-f <val> ] File path to be played back(absolute path).\n\ +\t[-n <val> ] Number of playbacks(fake model), default: 1\n\ +\t[-r <val> ] The rate for playing fake data is only valid\ + when parameter 's' is used. default:10hz.\n\ +\t[-s <val> ] Playback fake data.\n\ +\t[-t <val> ] 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; +}
