Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-21 13:02:25 +08:00
Code Issues Actions 4 Packages Projects Releases Wiki Activity

listener: add optional rate and cleanup existing arguments

Browse Source
This commit is contained in:
Daniel Agar
2018-07-15 15:38:23 -04:00
committed by Beat Küng
parent 5cac32de66
commit 2302c64004
6 changed files with 245 additions and 84 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Tools/uorb_graph/create.py
+1
View File
@@ -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\]$'),
src/systemcmds/topic_listener/CMakeLists.txt
+8 -4
View File
@@ -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
)
src/systemcmds/topic_listener/generate_listener.py
+13 -58
View File
@@ -99,78 +99,33 @@ print("""
#include <stdlib.h>
#include <inttypes.h>
#ifndef PRIu64
#define PRIu64 "llu"
#endif
#ifndef PRId64
#define PRId64 "lld"
#endif
#include <topic_listener.hpp>
#include <topic_listener_generated.hpp>
""")
for m in set(messages):
print("#include <uORB/topics/%s.h>" % 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")
src/systemcmds/topic_listener/listener_main.cpp
+118
View File
@@ -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 <px4_module.h>
#include <px4_getopt.h>
#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("<topic_name>", "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);
}
src/systemcmds/topic_listener/topic_listener.hpp
+102
View File
@@ -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 <px4_defines.h>
#include <drivers/drv_hrt.h>
#include <px4_middleware.h>
#include <px4_app.h>
#include <px4_config.h>
#include <uORB/uORB.h>
#include <string.h>
#include <stdint.h>
#include <stdio.h>
#include <stdlib.h>
#include <inttypes.h>
template <typename T>
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);
}
src/systemcmds/topic_listener/usage.cpp → src/systemcmds/topic_listener/topic_listener_generated.hpp
+3 -22
View File
@@ -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 <px4_module.h>
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("<topic_name> [<num_msgs>]", "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);