diff --git a/Tools/uorb_graph/create.py b/Tools/uorb_graph/create.py index 938fdc3aa1..f30507bd58 100755 --- a/Tools/uorb_graph/create.py +++ b/Tools/uorb_graph/create.py @@ -222,6 +222,7 @@ class Graph: special_cases_sub = [ ('sensors', r'voted_sensors_update\.cpp$', r'\binit_sensor_class\b\(([^,)]+)', r'^meta$'), ('mavlink', r'.*', r'\badd_orb_subscription\b\(([^,)]+)', r'^_topic$'), + ('listener', r'.*', None, r'^(id)$'), ('logger', r'.*', None, r'^(topic|sub\.metadata|_polling_topic_meta)$'), ('uavcan', r'uavcan_main\.cpp$', r'\b_control_topics\[[0-9]\]=([^,)]+)', r'^_control_topics\[i\]$'), diff --git a/src/systemcmds/topic_listener/CMakeLists.txt b/src/systemcmds/topic_listener/CMakeLists.txt index 014991b6c7..3b759e8cec 100644 --- a/src/systemcmds/topic_listener/CMakeLists.txt +++ b/src/systemcmds/topic_listener/CMakeLists.txt @@ -31,14 +31,14 @@ # ############################################################################ -add_custom_command(OUTPUT topic_listener.cpp - COMMAND ${PYTHON_EXECUTABLE} ${CMAKE_CURRENT_SOURCE_DIR}/generate_listener.py ${PX4_SOURCE_DIR} ${EXTERNAL_MODULES_LOCATION} > topic_listener.cpp +add_custom_command(OUTPUT listener_generated.cpp + COMMAND ${PYTHON_EXECUTABLE} ${CMAKE_CURRENT_SOURCE_DIR}/generate_listener.py ${PX4_SOURCE_DIR} ${EXTERNAL_MODULES_LOCATION} > listener_generated.cpp DEPENDS generate_listener.py uorb_msgs ) add_custom_target(generate_topic_listener DEPENDS - topic_listener.cpp + listener_generated.cpp generate_listener.py uorb_msgs ) @@ -48,8 +48,12 @@ px4_add_module( MAIN listener STACK_MAIN 1800 COMPILE_FLAGS + INCLUDES + ${CMAKE_CURRENT_SOURCE_DIR} + ${CMAKE_CURRENT_BINARY_DIR} SRCS - topic_listener.cpp + listener_main.cpp + listener_generated.cpp DEPENDS generate_topic_listener ) diff --git a/src/systemcmds/topic_listener/generate_listener.py b/src/systemcmds/topic_listener/generate_listener.py index 351a322c02..18fefacf1e 100755 --- a/src/systemcmds/topic_listener/generate_listener.py +++ b/src/systemcmds/topic_listener/generate_listener.py @@ -99,78 +99,33 @@ print(""" #include #include -#ifndef PRIu64 -#define PRIu64 "llu" -#endif - -#ifndef PRId64 -#define PRId64 "lld" -#endif +#include +#include """) for m in set(messages): print("#include " % m) -print(""" -extern "C" __EXPORT int listener_main(int argc, char *argv[]); - -static bool check_timeout(const hrt_abstime& time) { - if (hrt_elapsed_time(&time) > 2*1000*1000) { - printf("Waited for 2 seconds without a message. Giving up.\\n"); - return true; - } - return false; -} +print (""" +\nvoid listener_generated(char * topic_name, int topic_instance, int topic_rate, int num_msgs) { """) -for index, (m, t) in enumerate(zip(messages, topics)): - print("void listen_%s(unsigned num_msgs, unsigned topic_instance);" % t) - -print (""" -\nint listener_main(int argc, char *argv[]) { - if(argc < 2) { - printf("need at least two arguments: topic name. [optional number of messages to print] [optional instance]\\n"); - return 1; +print(""" + unsigned topic_interval = 0; + if (topic_rate != 0) { + topic_interval = 1000 / topic_rate; } """) -print("\tunsigned num_msgs = (argc > 2) ? atoi(argv[2]) : 1;") -print("\tunsigned topic_instance = (argc > 3) ? atoi(argv[3]) : 0;\n") + for index, (m, t) in enumerate(zip(messages, topics)): if index == 0: - print("\tif (strncmp(argv[1],\"%s\",50) == 0) {" % t) + print("\tif (strncmp(topic_name,\"%s\", %d) == 0) {" % (t, len(t))) else: - print("\t} else if (strncmp(argv[1],\"%s\",50) == 0) {" % t) - print("\t\tlisten_%s(num_msgs, topic_instance);" % t) + print("\t} else if (strcmp(topic_name,\"%s\") == 0) {" % (t)) + print("\t\tlistener<%s_s>(ORB_ID(%s), num_msgs, topic_instance, topic_interval);" % (m, t)) print("\t} else {") print("\t\t printf(\" Topic did not match any known topics\\n\");") print("\t}") -print("\t return 0;") -print("}\n") -for index, (m, t) in enumerate(zip(messages, topics)): - print("void listen_%s(unsigned num_msgs, unsigned topic_instance) {" % t) - print("\torb_id_t ID = ORB_ID(%s);" % t) - print("\tif (orb_exists(ID, topic_instance) != 0) { printf(\"never published\\n\"); return; }") - print("\tint sub = orb_subscribe_multi(ORB_ID(%s), topic_instance);" % t) - print("\tbool updated = false;") - print("\tunsigned i = 0;") - print("\thrt_abstime start_time = hrt_absolute_time();") - print("\twhile(i < num_msgs) {") - print("\t\torb_check(sub, &updated);") - print("\t\tif (i == 0) { updated = true; } else { usleep(500); }") - print("\t\tif (updated) {") - print("\t\t\tstart_time = hrt_absolute_time();") - print("\t\t\ti++;") - print("\t\tprintf(\"\\nTOPIC: %s instance %%d #%%d\\n\", topic_instance, i);" % t) - print("\t\t%s_s container = {};" % m) - print("\t\torb_copy(ID, sub, &container);") - print("\t\tprint_message(container);") - print("\t\t} else {") - print("\t\t\tif (check_timeout(start_time)) {") - print("\t\t\t\tbreak;") - print("\t\t\t}") - print("\t\t}") - print("\t}") - print("\torb_unsubscribe(sub);") - print("}\n") +print("}\n") diff --git a/src/systemcmds/topic_listener/listener_main.cpp b/src/systemcmds/topic_listener/listener_main.cpp new file mode 100644 index 0000000000..030ded3cb9 --- /dev/null +++ b/src/systemcmds/topic_listener/listener_main.cpp @@ -0,0 +1,118 @@ +/**************************************************************************** + * + * Copyright (c) 2018 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file listener_main.cpp + * + * Tool for listening to topics. + */ + +#include +#include + +#include "topic_listener.hpp" +#include "topic_listener_generated.hpp" + +extern "C" __EXPORT int listener_main(int argc, char *argv[]); + +static void usage(); + +int listener_main(int argc, char *argv[]) +{ + if (argc <= 1) { + usage(); + return 1; + } + + char *topic_name = argv[1]; + + unsigned topic_instance = 0; + unsigned topic_rate = 0; + unsigned num_msgs = 0; + + int myoptind = 1; + int ch; + const char *myoptarg = nullptr; + + while ((ch = px4_getopt(argc, argv, "i:r:n:", &myoptind, &myoptarg)) != EOF) { + switch (ch) { + + case 'i': + topic_instance = strtol(myoptarg, nullptr, 0); + break; + + case 'r': + topic_rate = strtol(myoptarg, nullptr, 0); + break; + + case 'n': + num_msgs = strtol(myoptarg, nullptr, 0); + break; + + default: + usage(); + return -1; + break; + } + } + + if (num_msgs == 0) { + if (topic_rate != 0) { + num_msgs = 10 * topic_rate; // arbitrary limit (10 seconds at max rate) + + } else { + num_msgs = 1; + } + } + + listener_generated(topic_name, topic_instance, topic_rate, num_msgs); + + return 0; +} + + +static void +usage() +{ + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +Utility to listen on uORB topics and print the data to the console. +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("listener", "command"); + PRINT_MODULE_USAGE_ARG("", "uORB topic name", false); + + PRINT_MODULE_USAGE_PARAM_INT('i', 0, 0, ORB_MULTI_MAX_INSTANCES - 1, "Topic instance", true); + PRINT_MODULE_USAGE_PARAM_INT('n', 1, 0, 100, "Number of messages", true); + PRINT_MODULE_USAGE_PARAM_INT('r', 0, 0, 1000, "Subscription rate (unlimited if 0)", true); +} diff --git a/src/systemcmds/topic_listener/topic_listener.hpp b/src/systemcmds/topic_listener/topic_listener.hpp new file mode 100644 index 0000000000..239ed02e21 --- /dev/null +++ b/src/systemcmds/topic_listener/topic_listener.hpp @@ -0,0 +1,102 @@ +/**************************************************************************** + * + * Copyright (c) 2018 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file listener.hpp + * + */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +template +void listener(const orb_id_t &id, unsigned num_msgs, unsigned topic_instance, unsigned topic_interval) +{ + if (orb_exists(id, topic_instance) != 0) { + printf("never published\n"); + return; + } + + int sub = orb_subscribe_multi(id, topic_instance); + orb_set_interval(sub, topic_interval); + + bool updated = false; + unsigned i = 0; + hrt_abstime start_time = hrt_absolute_time(); + + while (i < num_msgs) { + orb_check(sub, &updated); + + if (i == 0) { + updated = true; + + } else { + usleep(500); + } + + if (updated) { + start_time = hrt_absolute_time(); + i++; + + printf("\nTOPIC: %s instance %d #%d\n", id->o_name, topic_instance, i); + + T container; + + if (orb_copy(id, sub, &container) == PX4_OK) { + print_message(container); + + } else { + PX4_ERR("orb_copy failed"); + } + + } else { + if (hrt_elapsed_time(&start_time) > 2 * 1000 * 1000) { + printf("Waited for 2 seconds without a message. Giving up.\n"); + break; + } + } + } + + orb_unsubscribe(sub); +} diff --git a/src/systemcmds/topic_listener/usage.cpp b/src/systemcmds/topic_listener/topic_listener_generated.hpp similarity index 73% rename from src/systemcmds/topic_listener/usage.cpp rename to src/systemcmds/topic_listener/topic_listener_generated.hpp index d4cc73e5d3..195b0ac227 100644 --- a/src/systemcmds/topic_listener/usage.cpp +++ b/src/systemcmds/topic_listener/topic_listener_generated.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2017 PX4 Development Team. All rights reserved. + * Copyright (c) 2018 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -31,25 +31,6 @@ * ****************************************************************************/ -/** - * @file usage.cpp - * Note: this file is not used in the build, it's just for documenting the module - */ - -#include - -static void -usage() -{ - PRINT_MODULE_DESCRIPTION( - R"DESCR_STR( -Utility to listen on uORB topics and print the data to the console. - -Limitation: it can only listen to the first instance of a topic. - -)DESCR_STR"); - - PRINT_MODULE_USAGE_NAME_SIMPLE("listener", "command"); - PRINT_MODULE_USAGE_ARG(" []", "uORB topic name and optionally number of messages (default=1)", false); -} +#pragma once +void listener_generated(char *topic_name, int topic_instance, int topic_rate, int num_msgs);