From 1a5bd6c906d2cae6b1db6c2711b0c0fb1a199f58 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Thu, 9 Jul 2015 01:03:58 +0300 Subject: [PATCH] Node monitor update --- libuavcan_drivers/linux/CMakeLists.txt | 6 +- .../linux/apps/uavcan_monitor.cpp | 185 ++++++++++++++++++ .../linux/apps/uavcan_status_monitor.cpp | 167 ---------------- 3 files changed, 188 insertions(+), 170 deletions(-) create mode 100644 libuavcan_drivers/linux/apps/uavcan_monitor.cpp delete mode 100644 libuavcan_drivers/linux/apps/uavcan_status_monitor.cpp diff --git a/libuavcan_drivers/linux/CMakeLists.txt b/libuavcan_drivers/linux/CMakeLists.txt index 463cdf9e7d..52779a2fbf 100644 --- a/libuavcan_drivers/linux/CMakeLists.txt +++ b/libuavcan_drivers/linux/CMakeLists.txt @@ -72,8 +72,8 @@ target_link_libraries(test_multithreading ${UAVCAN_LIB} rt ${CMAKE_THREAD_LIBS_I # # Tools # -add_executable(uavcan_status_monitor apps/uavcan_status_monitor.cpp) -target_link_libraries(uavcan_status_monitor ${UAVCAN_LIB} rt ${CMAKE_THREAD_LIBS_INIT}) +add_executable(uavcan_monitor apps/uavcan_monitor.cpp) +target_link_libraries(uavcan_monitor ${UAVCAN_LIB} rt ${CMAKE_THREAD_LIBS_INIT}) add_executable(uavcan_nodetool apps/uavcan_nodetool.cpp) target_link_libraries(uavcan_nodetool ${UAVCAN_LIB} rt ${CMAKE_THREAD_LIBS_INIT}) @@ -81,7 +81,7 @@ target_link_libraries(uavcan_nodetool ${UAVCAN_LIB} rt ${CMAKE_THREAD_LIBS_INIT} add_executable(uavcan_dynamic_node_id_server apps/uavcan_dynamic_node_id_server.cpp) target_link_libraries(uavcan_dynamic_node_id_server ${UAVCAN_LIB} rt ${CMAKE_THREAD_LIBS_INIT}) -install(TARGETS uavcan_status_monitor +install(TARGETS uavcan_monitor uavcan_nodetool uavcan_dynamic_node_id_server RUNTIME DESTINATION bin) diff --git a/libuavcan_drivers/linux/apps/uavcan_monitor.cpp b/libuavcan_drivers/linux/apps/uavcan_monitor.cpp new file mode 100644 index 0000000000..8fdee456ae --- /dev/null +++ b/libuavcan_drivers/linux/apps/uavcan_monitor.cpp @@ -0,0 +1,185 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include +#include +#include +#include +#include "debug.hpp" + +enum class CLIColor : unsigned +{ + Red = 31, + Green = 32, + Yellow = 33, + Blue = 34, + Magenta = 35, + Cyan = 36, + White = 37, + Default = 39 +}; + +class CLIColorizer +{ + const CLIColor color_; +public: + explicit CLIColorizer(CLIColor c) : color_(c) + { + std::printf("\033[%um", static_cast(color_)); + } + + ~CLIColorizer() + { + std::printf("\033[%um", static_cast(CLIColor::Default)); + } +}; + +class Monitor : public uavcan::NodeStatusMonitor +{ + uavcan_linux::TimerPtr timer_; + std::unordered_map status_registry_; + + void handleNodeStatusMessage(const uavcan::ReceivedDataStructure& msg) override + { + status_registry_[msg.getSrcNodeID().get()] = msg; + } + + static std::pair healthToColoredString(const std::uint8_t health) + { + static const std::unordered_map> map + { + { uavcan::protocol::NodeStatus::HEALTH_OK, { CLIColor(CLIColor::Green), "OK" }}, + { uavcan::protocol::NodeStatus::HEALTH_WARNING, { CLIColor(CLIColor::Yellow), "WARNING" }}, + { uavcan::protocol::NodeStatus::HEALTH_ERROR, { CLIColor(CLIColor::Magenta), "ERROR" }}, + { uavcan::protocol::NodeStatus::HEALTH_CRITICAL, { CLIColor(CLIColor::Red), "CRITICAL" }} + }; + try + { + return map.at(health); + } + catch (std::out_of_range&) + { + return { CLIColor(CLIColor::Red), std::to_string(health) }; + } + } + + static std::pair modeToColoredString(const std::uint8_t mode) + { + static const std::unordered_map> map + { + { uavcan::protocol::NodeStatus::MODE_OPERATIONAL, { CLIColor(CLIColor::Green), "OPERATIONAL" }}, + { uavcan::protocol::NodeStatus::MODE_INITIALIZATION, { CLIColor(CLIColor::Yellow), "INITIALIZATION" }}, + { uavcan::protocol::NodeStatus::MODE_MAINTENANCE, { CLIColor(CLIColor::Cyan), "MAINTENANCE" }}, + { uavcan::protocol::NodeStatus::MODE_SOFTWARE_UPDATE, { CLIColor(CLIColor::Magenta), "SOFTWARE_UPDATE" }}, + { uavcan::protocol::NodeStatus::MODE_OFFLINE, { CLIColor(CLIColor::Red), "OFFLINE" }} + }; + try + { + return map.at(mode); + } + catch (std::out_of_range&) + { + return { CLIColor(CLIColor::Red), std::to_string(mode) }; + } + } + + void printStatusLine(const uavcan::NodeID nid, const uavcan::NodeStatusMonitor::NodeStatus& status) + { + const auto health_and_color = healthToColoredString(status.health); + const auto mode_and_color = modeToColoredString(status.mode); + + const int nid_int = nid.get(); + const unsigned long uptime = status_registry_[nid_int].uptime_sec; + const unsigned vendor_code = status_registry_[nid_int].vendor_specific_status_code; + + std::printf(" %-3d |", nid_int); + { + CLIColorizer clz(mode_and_color.first); + std::printf(" %-15s ", mode_and_color.second.c_str()); + } + std::printf("|"); + { + CLIColorizer clz(health_and_color.first); + std::printf(" %-8s ", health_and_color.second.c_str()); + } + std::printf("| %-10lu | %04x %s'%s %u\n", uptime, + vendor_code, + std::bitset<8>((vendor_code >> 8) & 0xFF).to_string().c_str(), + std::bitset<8>(vendor_code).to_string().c_str(), + vendor_code); + } + + void redraw(const uavcan::TimerEvent&) + { + std::printf("\x1b[1J"); // Clear screen from the current cursor position to the beginning + std::printf("\x1b[H"); // Move cursor to the coordinates 1,1 + std::printf(" NID | Mode | Health | Uptime [s] | Vendor-specific status code\n"); + std::printf("-----+-----------------+----------+------------+-hex---bin----------------dec--\n"); + + for (unsigned i = 1; i <= uavcan::NodeID::Max; i++) + { + if (isNodeKnown(i)) + { + printStatusLine(i, getNodeStatus(i)); + } + } + } + +public: + explicit Monitor(uavcan_linux::NodePtr node) + : uavcan::NodeStatusMonitor(*node) + , timer_(node->makeTimer(uavcan::MonotonicDuration::fromMSec(500), + std::bind(&Monitor::redraw, this, std::placeholders::_1))) + { } +}; + + +static uavcan_linux::NodePtr initNodeInPassiveMode(const std::vector& ifaces, const std::string& node_name) +{ + auto node = uavcan_linux::makeNode(ifaces); + node->setName(node_name.c_str()); + ENFORCE(0 == node->start()); + node->setModeOperational(); + return node; +} + +static void runForever(const uavcan_linux::NodePtr& node) +{ + Monitor mon(node); + ENFORCE(0 == mon.start()); + while (true) + { + const int res = node->spin(uavcan::MonotonicDuration::getInfinite()); + if (res < 0) + { + node->logError("spin", "Error %*", res); + } + } +} + +int main(int argc, const char** argv) +{ + try + { + if (argc < 2) + { + std::cerr << "Usage:\n\t" << argv[0] << " [can-iface-name-N...]" << std::endl; + return 1; + } + std::vector iface_names; + for (int i = 1; i < argc; i++) + { + iface_names.emplace_back(argv[i]); + } + uavcan_linux::NodePtr node = initNodeInPassiveMode(iface_names, "org.uavcan.linux_app.node_status_monitor"); + runForever(node); + return 0; + } + catch (const std::exception& ex) + { + std::cerr << "Error: " << ex.what() << std::endl; + return 1; + } +} diff --git a/libuavcan_drivers/linux/apps/uavcan_status_monitor.cpp b/libuavcan_drivers/linux/apps/uavcan_status_monitor.cpp deleted file mode 100644 index bfcfa46753..0000000000 --- a/libuavcan_drivers/linux/apps/uavcan_status_monitor.cpp +++ /dev/null @@ -1,167 +0,0 @@ -/* - * Copyright (C) 2014 Pavel Kirienko - */ - -#include -#include -#include -#include -#include -#include -#include "debug.hpp" - -struct OstreamColorizer -{ - enum Color - { - Red = 31, - Green = 32, - Yellow = 33, - Blue = 34, - Magenta = 35, - Cyan = 36, - Default = 39 - }; - explicit OstreamColorizer(Color color = Default) : color_(color) { } - friend std::ostream& operator<<(std::ostream& os, const OstreamColorizer& mod) - { - return os << "\033[" << int(mod.color_) << "m"; - } -private: - const Color color_; -}; - - -class Monitor : public uavcan::NodeStatusMonitor -{ - uavcan_linux::TimerPtr timer_; - std::unordered_map status_registry_; - - void handleNodeStatusMessage(const uavcan::ReceivedDataStructure& msg) override - { - status_registry_[msg.getSrcNodeID().get()] = msg; - } - - static std::pair - statusCodeToColoredString(uavcan::NodeStatusMonitor::NodeStatusCode status_code) - { - if (status_code == uavcan::protocol::NodeStatus::STATUS_OK) - { - return { OstreamColorizer(OstreamColorizer::Green), "OK" }; - } - if (status_code == uavcan::protocol::NodeStatus::STATUS_INITIALIZING) - { - return { OstreamColorizer(OstreamColorizer::Cyan), "INITIALIZING" }; - } - if (status_code == uavcan::protocol::NodeStatus::STATUS_WARNING) - { - return { OstreamColorizer(OstreamColorizer::Yellow), "WARNING" }; - } - if (status_code == uavcan::protocol::NodeStatus::STATUS_CRITICAL) - { - return { OstreamColorizer(OstreamColorizer::Red), "CRITICAL" }; - } - if (status_code == uavcan::protocol::NodeStatus::STATUS_OFFLINE) - { - return { OstreamColorizer(OstreamColorizer::Magenta), "OFFLINE" }; - } - return { OstreamColorizer(), "???" }; - } - - void printStatusLine(uavcan::NodeID nid, const uavcan::NodeStatusMonitor::NodeStatus& status) - { - const auto original_flags = std::cout.flags(); - - const auto color_and_string = statusCodeToColoredString(status.status_code); - const int nid_int = nid.get(); - const auto uptime = status_registry_[nid_int].uptime_sec; - const int vendor_code = status_registry_[nid_int].vendor_specific_status_code; - - std::cout << color_and_string.first; - - std::cout << " " << std::setw(3) << std::left << nid_int << " | " // Node ID - << std::setw(13) << std::left << color_and_string.second << " | " // Status name - << std::setw(12) << uptime << " | " // Uptime - << "0x" << std::hex << std::setw(4) << std::setfill('0') << vendor_code // Vendor, hex - << " 0b" << std::dec << std::bitset<8>((vendor_code >> 8) & 0xFF) // Vendor, bin, high - << "'" << std::bitset<8>(vendor_code & 0xFF) // Vendor, bin, low - << " " << vendor_code; // Vendor, dec - - std::cout << OstreamColorizer() << std::setfill(' ') << "\n"; - std::cout.width(0); - std::cout.flags(original_flags); - } - - void redraw(const uavcan::TimerEvent&) - { - std::cout << "\x1b[1J" // Clear screen from the current cursor position to the beginning - << "\x1b[H" // Move cursor to the coordinates 1,1 - << " NID | Status | Uptime (sec) | Vendor-specific status (hex/bin/dec)\n" - << "-----+---------------+--------------+--------------------------------------\n"; - for (unsigned i = 1; i <= uavcan::NodeID::Max; i++) - { - const auto s = getNodeStatus(i); - if (s.known) - { - printStatusLine(i, s); - } - } - std::cout << std::flush; - } - -public: - explicit Monitor(uavcan_linux::NodePtr node) - : uavcan::NodeStatusMonitor(*node) - , timer_(node->makeTimer(uavcan::MonotonicDuration::fromMSec(500), - std::bind(&Monitor::redraw, this, std::placeholders::_1))) - { } -}; - - -static uavcan_linux::NodePtr initNodeInPassiveMode(const std::vector& ifaces, const std::string& node_name) -{ - auto node = uavcan_linux::makeNode(ifaces); - node->setName(node_name.c_str()); - ENFORCE(0 == node->start()); - node->setStatusOk(); - return node; -} - -static void runForever(const uavcan_linux::NodePtr& node) -{ - Monitor mon(node); - ENFORCE(0 == mon.start()); - while (true) - { - const int res = node->spin(uavcan::MonotonicDuration::getInfinite()); - if (res < 0) - { - node->logError("spin", "Error %*", res); - } - } -} - -int main(int argc, const char** argv) -{ - try - { - if (argc < 2) - { - std::cerr << "Usage:\n\t" << argv[0] << " [can-iface-name-N...]" << std::endl; - return 1; - } - std::vector iface_names; - for (int i = 1; i < argc; i++) - { - iface_names.emplace_back(argv[i]); - } - uavcan_linux::NodePtr node = initNodeInPassiveMode(iface_names, "org.uavcan.linux_app.node_status_monitor"); - runForever(node); - return 0; - } - catch (const std::exception& ex) - { - std::cerr << "Error: " << ex.what() << std::endl; - return 1; - } -}