Diff: libuavcan_drivers/linux/apps/uavcan_monitor.cpp

Revision:

0:dfe6edabb8ec

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan_drivers/linux/apps/uavcan_monitor.cpp	Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,183 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include <cstdio>
+#include <bitset>
+#include <unordered_map>
+#include <uavcan_linux/uavcan_linux.hpp>
+#include <uavcan/protocol/node_status_monitor.hpp>
+#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<unsigned>(color_));
+    }
+
+    ~CLIColorizer()
+    {
+        std::printf("\033[%um", static_cast<unsigned>(CLIColor::Default));
+    }
+};
+
+class Monitor : public uavcan::NodeStatusMonitor
+{
+    uavcan_linux::TimerPtr timer_;
+    std::unordered_map<int, uavcan::protocol::NodeStatus> status_registry_;
+
+    void handleNodeStatusMessage(const uavcan::ReceivedDataStructure<uavcan::protocol::NodeStatus>& msg) override
+    {
+        status_registry_[msg.getSrcNodeID().get()] = msg;
+    }
+
+    static std::pair<CLIColor, std::string> healthToColoredString(const std::uint8_t health)
+    {
+        static const std::unordered_map<std::uint8_t, std::pair<CLIColor, std::string>> 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<CLIColor, std::string> modeToColoredString(const std::uint8_t mode)
+    {
+        static const std::unordered_map<std::uint8_t, std::pair<CLIColor, std::string>> 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<std::string>& ifaces, const std::string& node_name)
+{
+    auto node = uavcan_linux::makeNode(ifaces, node_name.c_str(),
+                                       uavcan::protocol::SoftwareVersion(), uavcan::protocol::HardwareVersion());
+    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-1> [can-iface-name-N...]" << std::endl;
+            return 1;
+        }
+        std::vector<std::string> 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;
+    }
+}