diff --git a/src/systemcmds/topic_listener/generate_listener.py b/src/systemcmds/topic_listener/generate_listener.py index 54cb564ba4e5..8b7ca12344b6 100755 --- a/src/systemcmds/topic_listener/generate_listener.py +++ b/src/systemcmds/topic_listener/generate_listener.py @@ -122,7 +122,7 @@ if index == 0: print("\tif (strncmp(topic_name,\"%s\", %d) == 0) {" % (t, len(t))) else: - print("\t} else if (strcmp(topic_name,\"%s\") == 0) {" % (t)) + print("\t} else if (strncmp(topic_name,\"%s\", %d) == 0) {" % (t, len(t))) print("\t\tlistener(listener_print_topic<%s_s>, ORB_ID(%s), num_msgs, topic_instance, topic_interval);" % (m, t)) print("\t} else {") diff --git a/src/systemcmds/topic_listener/listener_main.cpp b/src/systemcmds/topic_listener/listener_main.cpp index daae9cc7ff6e..5236a5715cd1 100644 --- a/src/systemcmds/topic_listener/listener_main.cpp +++ b/src/systemcmds/topic_listener/listener_main.cpp @@ -49,77 +49,111 @@ extern "C" __EXPORT int listener_main(int argc, char *argv[]); static void usage(); -void listener(listener_print_topic_cb cb, const orb_id_t &id, unsigned num_msgs, unsigned topic_instance, +void listener(listener_print_topic_cb cb, const orb_id_t &id, unsigned num_msgs, int topic_instance, unsigned topic_interval) { - if (orb_exists(id, topic_instance) != 0) { - PX4_INFO_RAW("never published\n"); - return; - } - int sub = orb_subscribe_multi(id, topic_instance); - orb_set_interval(sub, topic_interval); + if (topic_instance == -1 && num_msgs == 1) { + // first count the number of instances + int instances = 0; - bool updated = false; - unsigned i = 0; - hrt_abstime start_time = hrt_absolute_time(); + for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) { + if (orb_exists(id, i) == PX4_OK) { + instances++; + } + } - while (i < num_msgs) { + if (instances == 1) { + PX4_INFO_RAW("\nTOPIC: %s\n", id->o_name); + int sub = orb_subscribe(id); + cb(id, sub); + orb_unsubscribe(sub); + + } else if (instances > 1) { + PX4_INFO_RAW("\nTOPIC: %s %d instances\n", id->o_name, instances); + + for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) { + if (orb_exists(id, i) == PX4_OK) { + PX4_INFO_RAW("\nInstance %d:\n", i); + int sub = orb_subscribe_multi(id, i); + cb(id, sub); + orb_unsubscribe(sub); + } + } + } - // check for user input to quit - int user_input_timeout = 1; + } else { + if (orb_exists(id, topic_instance) != 0) { + PX4_INFO_RAW("never published\n"); + return; + } - orb_check(sub, &updated); + int sub = orb_subscribe_multi(id, topic_instance); + orb_set_interval(sub, topic_interval); - if (i == 0) { - updated = true; - user_input_timeout = 0; // don't wait - } + bool updated = false; + unsigned i = 0; + hrt_abstime start_time = hrt_absolute_time(); - // check for user input - struct pollfd fds {}; - fds.fd = 0; /* stdin */ - fds.events = POLLIN; + while (i < num_msgs) { - if (poll(&fds, 1, 0) > 0) { + // check for user input to quit + int user_input_timeout = 1; - char c = 0; - int ret = read(0, &c, user_input_timeout); + orb_check(sub, &updated); - if (ret) { - return; + if (i == 0) { + updated = true; + user_input_timeout = 0; // don't wait } - switch (c) { - case 0x03: // ctrl-c - case 0x1b: // esc - case 'q': - return; - /* not reached */ - } - } + // check for user input + struct pollfd fds {}; + fds.fd = 0; /* stdin */ + fds.events = POLLIN; - if (updated) { - start_time = hrt_absolute_time(); - i++; + if (poll(&fds, 1, 0) > 0) { - PX4_INFO_RAW("\nTOPIC: %s instance %d #%d\n", id->o_name, topic_instance, i); + char c = 0; + int ret = read(0, &c, user_input_timeout); - int ret = cb(id, sub); + if (ret) { + return; + } - if (ret != PX4_OK) { - PX4_ERR("listener callback failed (%i)", ret); + switch (c) { + case 0x03: // ctrl-c + case 0x1b: // esc + case 'q': + return; + /* not reached */ + } } - } else { - if (hrt_elapsed_time(&start_time) > 2 * 1000 * 1000) { - PX4_INFO_RAW("Waited for 2 seconds without a message. Giving up.\n"); - break; + if (updated) { + start_time = hrt_absolute_time(); + i++; + + PX4_INFO_RAW("\nTOPIC: %s instance %d #%d\n", id->o_name, topic_instance, i); + + int ret = cb(id, sub); + + if (ret != PX4_OK) { + PX4_ERR("listener callback failed (%i)", ret); + } + + } else { + if (hrt_elapsed_time(&start_time) > 2 * 1000 * 1000) { + PX4_INFO_RAW("Waited for 2 seconds without a message. Giving up.\n"); + break; + } } } + + orb_unsubscribe(sub); } - orb_unsubscribe(sub); + } int listener_main(int argc, char *argv[]) @@ -131,7 +165,7 @@ int listener_main(int argc, char *argv[]) char *topic_name = argv[1]; - unsigned topic_instance = 0; + int topic_instance = -1; unsigned topic_rate = 0; unsigned num_msgs = 0; diff --git a/src/systemcmds/topic_listener/topic_listener.hpp b/src/systemcmds/topic_listener/topic_listener.hpp index 7a4c17a68469..ce856bd8e80b 100644 --- a/src/systemcmds/topic_listener/topic_listener.hpp +++ b/src/systemcmds/topic_listener/topic_listener.hpp @@ -67,5 +67,5 @@ int listener_print_topic(const orb_id_t &orb_id, int subscription) return ret; } -void listener(listener_print_topic_cb cb, const orb_id_t &id, unsigned num_msgs, unsigned topic_instance, +void listener(listener_print_topic_cb cb, const orb_id_t &id, unsigned num_msgs, int topic_instance, unsigned topic_interval);