From fbd7aac4b580bf88160d34b70ecece4f3222ab13 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Thu, 8 Sep 2016 13:32:09 +0200 Subject: [PATCH] uorb: add 'top' command for a live view of topic updates --- src/modules/uORB/uORB.h | 2 +- src/modules/uORB/uORBDevices_nuttx.cpp | 171 +++++++++++++++++++++++++ src/modules/uORB/uORBDevices_nuttx.hpp | 25 ++++ src/modules/uORB/uORBDevices_posix.cpp | 168 ++++++++++++++++++++++++ src/modules/uORB/uORBDevices_posix.hpp | 25 ++++ src/modules/uORB/uORBMain.cpp | 15 ++- 6 files changed, 404 insertions(+), 2 deletions(-) diff --git a/src/modules/uORB/uORB.h b/src/modules/uORB/uORB.h index 9f7e92f65d..d4633c7d61 100644 --- a/src/modules/uORB/uORB.h +++ b/src/modules/uORB/uORB.h @@ -61,7 +61,7 @@ typedef const struct orb_metadata *orb_id_t; /** * Maximum number of multi topic instances */ -#define ORB_MULTI_MAX_INSTANCES 4 +#define ORB_MULTI_MAX_INSTANCES 4 // This must be < 10 (because it's the last char of the node path) /** * Topic priority. diff --git a/src/modules/uORB/uORBDevices_nuttx.cpp b/src/modules/uORB/uORBDevices_nuttx.cpp index 676043a79f..69d5347749 100644 --- a/src/modules/uORB/uORBDevices_nuttx.cpp +++ b/src/modules/uORB/uORBDevices_nuttx.cpp @@ -833,6 +833,177 @@ void uORB::DeviceMaster::printStatistics(bool reset) } } +void uORB::DeviceMaster::addNewDeviceNodes(DeviceNodeStatisticsData **first_node, int &num_topics, + size_t &max_topic_name_length, + char **topic_filter, int num_filters) +{ + DeviceNodeStatisticsData *cur_node; + num_topics = 0; + DeviceNodeStatisticsData *last_node = *first_node; + + if (last_node) { + while (last_node->next) { + last_node = last_node->next; + } + } + + + for (ORBMap::Node *node = _node_map.top(); node; node = node->next) { + ++num_topics; + + //check if already added + cur_node = *first_node; + + while (cur_node && cur_node->node != node->node) { + cur_node = cur_node->next; + } + + if (cur_node) { + continue; + } + + if (num_filters > 0 && topic_filter) { + bool matched = false; + + for (int i = 0; i < num_filters; ++i) { + if (strstr(node->node->meta()->o_name, topic_filter[i])) { + matched = true; + } + } + + if (!matched) { + continue; + } + } + + if (last_node) { + last_node->next = new DeviceNodeStatisticsData(); + last_node = last_node->next; + + } else { + *first_node = last_node = new DeviceNodeStatisticsData(); + } + + if (!last_node) { + PX4_ERR("mem alloc failed"); + break; + } + + last_node->node = node->node; + int node_name_len = strlen(node->node_name); + last_node->instance = (uint8_t)(node->node_name[node_name_len - 1] - '0'); + size_t name_length = strlen(last_node->node->meta()->o_name); + + if (name_length > max_topic_name_length) { + max_topic_name_length = name_length; + } + + last_node->last_lost_msg_count = last_node->node->lost_message_count(); + last_node->last_pub_msg_count = last_node->node->published_message_count(); + + } +} + +#define CLEAR_LINE "\033[K" + +void uORB::DeviceMaster::showTop(char **topic_filter, int num_filters) +{ + + bool print_active_only = true; + + if (topic_filter && num_filters > 0) { + if (!strcmp("-a", topic_filter[0])) { + num_filters = 0; + } + + print_active_only = false; // print non-active if -a or some filter given + } + + printf("\033[2J\n"); //clear screen + + lock(); + + if (!_node_map.top()) { + unlock(); + PX4_INFO("no active topics"); + return; + } + + DeviceNodeStatisticsData *first_node = nullptr; + DeviceNodeStatisticsData *cur_node = nullptr; + size_t max_topic_name_length = 0; + int num_topics = 0; + addNewDeviceNodes(&first_node, num_topics, max_topic_name_length, topic_filter, num_filters); + + /* a DeviceNode is never deleted, so it's save to unlock here and still access the DeviceNodes */ + unlock(); + + struct pollfd fds; + fds.fd = 0; /* stdin */ + fds.events = POLLIN; + bool quit = false; + + while (!quit) { + /* Sleep 200 ms waiting for user input five times ~ 1s */ + for (int k = 0; k < 5; k++) { + char c; + + int ret = ::poll(&fds, 1, 0); //just want to check if there is new data available + + if (ret > 0) { + + ret = read(0, &c, 1); + + if (ret) { + quit = true; + break; + } + } + + usleep(200000); + } + + if (!quit) { + printf("\033[H"); // move cursor home and clear screen + printf(CLEAR_LINE "update: 1s, num topics: %i\n", num_topics); + printf(CLEAR_LINE "%*-s INST #SUB #MSG #LOST #QSIZE\n", (int)max_topic_name_length - 2, "TOPIC NAME"); + cur_node = first_node; + + while (cur_node) { + uint32_t num_lost = cur_node->node->lost_message_count(); + unsigned int num_msgs = cur_node->node->published_message_count(); + + if (!print_active_only || num_msgs - cur_node->last_pub_msg_count > 0) { + printf(CLEAR_LINE "%*-s %2i %4i %4i %5i %i\n", (int)max_topic_name_length, + cur_node->node->meta()->o_name, (int)cur_node->instance, + cur_node->node->subscriber_count(), num_msgs - cur_node->last_pub_msg_count, + num_lost - cur_node->last_lost_msg_count, cur_node->node->queue_size()); + } + + cur_node->last_lost_msg_count = num_lost; + cur_node->last_pub_msg_count = num_msgs; + + cur_node = cur_node->next; + } + + lock(); + addNewDeviceNodes(&first_node, num_topics, max_topic_name_length, topic_filter, num_filters); + unlock(); + } + } + + //cleanup + cur_node = first_node; + + while (cur_node) { + DeviceNodeStatisticsData *next_node = cur_node->next; + delete cur_node; + cur_node = next_node; + } +} + +#undef CLEAR_LINE + uORB::DeviceNode *uORB::DeviceMaster::getDeviceNode(const char *nodepath) { diff --git a/src/modules/uORB/uORBDevices_nuttx.hpp b/src/modules/uORB/uORBDevices_nuttx.hpp index dd371f6716..5703f58fcc 100644 --- a/src/modules/uORB/uORBDevices_nuttx.hpp +++ b/src/modules/uORB/uORBDevices_nuttx.hpp @@ -186,6 +186,12 @@ public: */ bool print_statistics(bool reset); + unsigned int queue_size() const { return _queue_size; } + int32_t subscriber_count() const { return _subscriber_count; } + uint32_t lost_message_count() const { return _lost_messages; } + unsigned int published_message_count() const { return _generation; } + const struct orb_metadata *meta() const { return _meta; } + protected: virtual pollevent_t poll_state(struct file *filp); virtual void poll_notify_one(struct pollfd *fds, pollevent_t events); @@ -280,11 +286,30 @@ public: */ void printStatistics(bool reset); + /** + * Continuously print statistics, like the unix top command for processes. + * Exited when the user presses the enter key. + * @param topic_filter list of topic filters: if set, each string can be a substring for topics to match. + * Or it can be '-a', which means to print all topics instead of only currently publishing ones. + * @param num_filters + */ + void showTop(char **topic_filter, int num_filters); + private: // Private constructor, uORB::Manager takes care of its creation DeviceMaster(Flavor f); virtual ~DeviceMaster(); + struct DeviceNodeStatisticsData { + DeviceNode *node; + uint8_t instance; + uint32_t last_lost_msg_count; + unsigned int last_pub_msg_count; + DeviceNodeStatisticsData *next = nullptr; + }; + void addNewDeviceNodes(DeviceNodeStatisticsData **first_node, int &num_topics, size_t &max_topic_name_length, + char **topic_filter, int num_filters); + friend class uORB::Manager; /** diff --git a/src/modules/uORB/uORBDevices_posix.cpp b/src/modules/uORB/uORBDevices_posix.cpp index 95b35dcce3..6dbba4394c 100644 --- a/src/modules/uORB/uORBDevices_posix.cpp +++ b/src/modules/uORB/uORBDevices_posix.cpp @@ -836,6 +836,174 @@ void uORB::DeviceMaster::printStatistics(bool reset) } } +void uORB::DeviceMaster::addNewDeviceNodes(DeviceNodeStatisticsData **first_node, int &num_topics, + size_t &max_topic_name_length, + char **topic_filter, int num_filters) +{ + DeviceNodeStatisticsData *cur_node; + num_topics = 0; + DeviceNodeStatisticsData *last_node = *first_node; + + if (last_node) { + while (last_node->next) { + last_node = last_node->next; + } + } + + for (const auto &node : _node_map) { + ++num_topics; + + //check if already added + cur_node = *first_node; + + while (cur_node && cur_node->node != node.second) { + cur_node = cur_node->next; + } + + if (cur_node) { + continue; + } + + if (num_filters > 0 && topic_filter) { + bool matched = false; + + for (int i = 0; i < num_filters; ++i) { + if (strstr(node.second->meta()->o_name, topic_filter[i])) { + matched = true; + } + } + + if (!matched) { + continue; + } + } + + if (last_node) { + last_node->next = new DeviceNodeStatisticsData(); + last_node = last_node->next; + + } else { + *first_node = last_node = new DeviceNodeStatisticsData(); + } + + if (!last_node) { + PX4_ERR("mem alloc failed"); + break; + } + + last_node->node = node.second; + last_node->instance = (uint8_t)(node.first[node.first.length() - 1] - '0'); + size_t name_length = strlen(last_node->node->meta()->o_name); + + if (name_length > max_topic_name_length) { + max_topic_name_length = name_length; + } + + last_node->last_lost_msg_count = last_node->node->lost_message_count(); + last_node->last_pub_msg_count = last_node->node->published_message_count(); + } +} + +#define CLEAR_LINE "\033[K" + +void uORB::DeviceMaster::showTop(char **topic_filter, int num_filters) +{ + + bool print_active_only = true; + + if (topic_filter && num_filters > 0) { + if (!strcmp("-a", topic_filter[0])) { + num_filters = 0; + } + + print_active_only = false; // print non-active if -a or some filter given + } + + printf("\033[2J\n"); //clear screen + + lock(); + + if (_node_map.empty()) { + unlock(); + PX4_INFO("no active topics"); + return; + } + + DeviceNodeStatisticsData *first_node = nullptr; + DeviceNodeStatisticsData *cur_node = nullptr; + size_t max_topic_name_length = 0; + int num_topics = 0; + addNewDeviceNodes(&first_node, num_topics, max_topic_name_length, topic_filter, num_filters); + + /* a DeviceNode is never deleted, so it's save to unlock here and still access the DeviceNodes */ + unlock(); + + struct pollfd fds; + fds.fd = 0; /* stdin */ + fds.events = POLLIN; + bool quit = false; + + while (!quit) { + /* Sleep 200 ms waiting for user input five times ~ 1s */ + for (int k = 0; k < 5; k++) { + char c; + + int ret = ::poll(&fds, 1, 0); //just want to check if there is new data available + + if (ret > 0) { + + ret = read(0, &c, 1); + + if (ret) { + quit = true; + break; + } + } + + usleep(200000); + } + + if (!quit) { + printf("\033[H"); // move cursor home and clear screen + printf(CLEAR_LINE "update: 1s, num topics: %i\n", num_topics); + printf(CLEAR_LINE "%*s INST #SUB #MSG #LOST #QSIZE\n", -(int)max_topic_name_length + 2, "TOPIC NAME"); + cur_node = first_node; + + while (cur_node) { + uint32_t num_lost = cur_node->node->lost_message_count(); + unsigned int num_msgs = cur_node->node->published_message_count(); + + if (!print_active_only || num_msgs - cur_node->last_pub_msg_count > 0) { + printf(CLEAR_LINE "%*s %2i %4i %4i %5i %i\n", -(int)max_topic_name_length, + cur_node->node->meta()->o_name, (int)cur_node->instance, + cur_node->node->subscriber_count(), num_msgs - cur_node->last_pub_msg_count, + num_lost - cur_node->last_lost_msg_count, cur_node->node->queue_size()); + } + + cur_node->last_lost_msg_count = num_lost; + cur_node->last_pub_msg_count = num_msgs; + + cur_node = cur_node->next; + } + + lock(); + addNewDeviceNodes(&first_node, num_topics, max_topic_name_length, topic_filter, num_filters); + unlock(); + } + } + + //cleanup + cur_node = first_node; + + while (cur_node) { + DeviceNodeStatisticsData *next_node = cur_node->next; + delete cur_node; + cur_node = next_node; + } +} + +#undef CLEAR_LINE + uORB::DeviceNode *uORB::DeviceMaster::getDeviceNode(const char *nodepath) { lock(); diff --git a/src/modules/uORB/uORBDevices_posix.hpp b/src/modules/uORB/uORBDevices_posix.hpp index 0d549f6fce..5fee2ef9b2 100644 --- a/src/modules/uORB/uORBDevices_posix.hpp +++ b/src/modules/uORB/uORBDevices_posix.hpp @@ -123,6 +123,12 @@ public: */ bool print_statistics(bool reset); + unsigned int queue_size() const { return _queue_size; } + int32_t subscriber_count() const { return _subscriber_count; } + uint32_t lost_message_count() const { return _lost_messages; } + unsigned int published_message_count() const { return _generation; } + const struct orb_metadata *meta() const { return _meta; } + protected: virtual pollevent_t poll_state(device::file_t *filp); virtual void poll_notify_one(px4_pollfd_struct_t *fds, pollevent_t events); @@ -216,11 +222,30 @@ public: */ void printStatistics(bool reset); + /** + * Continuously print statistics, like the unix top command for processes. + * Exited when the user presses the enter key. + * @param topic_filter list of topic filters: if set, each string can be a substring for topics to match. + * Or it can be '-a', which means to print all topics instead of only currently publishing ones. + * @param num_filters + */ + void showTop(char **topic_filter, int num_filters); + private: // Private constructor, uORB::Manager takes care of its creation DeviceMaster(Flavor f); virtual ~DeviceMaster(); + struct DeviceNodeStatisticsData { + DeviceNode *node; + uint8_t instance; + uint32_t last_lost_msg_count; + unsigned int last_pub_msg_count; + DeviceNodeStatisticsData *next = nullptr; + }; + void addNewDeviceNodes(DeviceNodeStatisticsData **first_node, int &num_topics, size_t &max_topic_name_length, + char **topic_filter, int num_filters); + friend class uORB::Manager; /** diff --git a/src/modules/uORB/uORBMain.cpp b/src/modules/uORB/uORBMain.cpp index af4fdf53db..8b287e4a1e 100644 --- a/src/modules/uORB/uORBMain.cpp +++ b/src/modules/uORB/uORBMain.cpp @@ -43,7 +43,9 @@ extern "C" { __EXPORT int uorb_main(int argc, char *argv[]); } static uORB::DeviceMaster *g_dev = nullptr; static void usage() { - PX4_INFO("Usage: uorb 'start', 'status'"); + PX4_INFO("Usage: uorb 'start', 'status', 'top [-a] [ [ ...]]'"); + PX4_INFO(" -a: print all instead of only currently publishing topics"); + PX4_INFO(" : topic(s) to match (implies -a)"); } int @@ -103,6 +105,17 @@ uorb_main(int argc, char *argv[]) return OK; } + if (!strcmp(argv[1], "top")) { + if (g_dev != nullptr) { + g_dev->showTop(argv + 2, argc - 2); + + } else { + PX4_INFO("uorb is not running"); + } + + return OK; + } + usage(); return -EINVAL; }