yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-22 01:12:31 +00:00
Code Issues Packages Projects Releases Wiki Activity

listener print all instances by default

Browse Source
This commit is contained in:
Daniel Agar
2019-08-24 10:25:29 -04:00
committed by Julian Oes
parent 693d89583d
commit 2f10c315b5
3 changed files with 103 additions and 69 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
src/systemcmds/topic_listener/generate_listener.py
View File

@@ -122,7 +122,7 @@ for index, (m, t) in enumerate(zip(messages, topics)):
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 {")

168
src/systemcmds/topic_listener/listener_main.cpp
View File

@@ -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;
if (topic_instance == -1 && num_msgs == 1) {
// first count the number of instances
int instances = 0;
for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) {
if (orb_exists(id, i) == PX4_OK) {
instances++;
}
}
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);
}
}
}
} else {
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);
bool updated = false;
unsigned i = 0;
hrt_abstime start_time = hrt_absolute_time();
while (i < num_msgs) {
// check for user input to quit
int user_input_timeout = 1;
orb_check(sub, &updated);
if (i == 0) {
updated = true;
user_input_timeout = 0; // don't wait
}
// check for user input
struct pollfd fds {};
fds.fd = 0; /* stdin */
fds.events = POLLIN;
if (poll(&fds, 1, 0) > 0) {
char c = 0;
int ret = read(0, &c, user_input_timeout);
if (ret) {
return;
}
switch (c) {
case 0x03: // ctrl-c
case 0x1b: // esc
case 'q':
return;
/* not reached */
}
}
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);
}
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) {
// check for user input to quit
int user_input_timeout = 1;
orb_check(sub, &updated);
if (i == 0) {
updated = true;
user_input_timeout = 0; // don't wait
}
// check for user input
struct pollfd fds {};
fds.fd = 0; /* stdin */
fds.events = POLLIN;
if (poll(&fds, 1, 0) > 0) {
char c = 0;
int ret = read(0, &c, user_input_timeout);
if (ret) {
return;
}
switch (c) {
case 0x03: // ctrl-c
case 0x1b: // esc
case 'q':
return;
/* not reached */
}
}
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);
}
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;

2
src/systemcmds/topic_listener/topic_listener.hpp
View File

@@ -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);