uavcan_monitor.cpp

00001 /*
00002  * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
00003  */
00004 
00005 #include <cstdio>
00006 #include <bitset>
00007 #include <unordered_map>
00008 #include <uavcan_linux/uavcan_linux.hpp>
00009 #include <uavcan/protocol/node_status_monitor.hpp>
00010 #include "debug.hpp"
00011 
00012 enum class CLIColor : unsigned
00013 {
00014     Red     = 31,
00015     Green   = 32,
00016     Yellow  = 33,
00017     Blue    = 34,
00018     Magenta = 35,
00019     Cyan    = 36,
00020     White   = 37,
00021     Default = 39
00022 };
00023 
00024 class CLIColorizer
00025 {
00026     const CLIColor color_;
00027 public:
00028     explicit CLIColorizer(CLIColor c) : color_(c)
00029     {
00030         std::printf("\033[%um", static_cast<unsigned>(color_));
00031     }
00032 
00033     ~CLIColorizer()
00034     {
00035         std::printf("\033[%um", static_cast<unsigned>(CLIColor::Default));
00036     }
00037 };
00038 
00039 class Monitor : public uavcan::NodeStatusMonitor
00040 {
00041     uavcan_linux::TimerPtr timer_;
00042     std::unordered_map<int, uavcan::protocol::NodeStatus> status_registry_;
00043 
00044     void handleNodeStatusMessage(const uavcan::ReceivedDataStructure<uavcan::protocol::NodeStatus>& msg) override
00045     {
00046         status_registry_[msg.getSrcNodeID().get()] = msg;
00047     }
00048 
00049     static std::pair<CLIColor, std::string> healthToColoredString(const std::uint8_t health)
00050     {
00051         static const std::unordered_map<std::uint8_t, std::pair<CLIColor, std::string>> map
00052         {
00053             { uavcan::protocol::NodeStatus::HEALTH_OK,       { CLIColor(CLIColor::Green),   "OK" }},
00054             { uavcan::protocol::NodeStatus::HEALTH_WARNING,  { CLIColor(CLIColor::Yellow),  "WARNING" }},
00055             { uavcan::protocol::NodeStatus::HEALTH_ERROR,    { CLIColor(CLIColor::Magenta), "ERROR" }},
00056             { uavcan::protocol::NodeStatus::HEALTH_CRITICAL, { CLIColor(CLIColor::Red),     "CRITICAL" }}
00057         };
00058         try
00059         {
00060             return map.at(health);
00061         }
00062         catch (std::out_of_range&)
00063         {
00064             return { CLIColor(CLIColor::Red), std::to_string(health) };
00065         }
00066     }
00067 
00068     static std::pair<CLIColor, std::string> modeToColoredString(const std::uint8_t mode)
00069     {
00070         static const std::unordered_map<std::uint8_t, std::pair<CLIColor, std::string>> map
00071         {
00072             { uavcan::protocol::NodeStatus::MODE_OPERATIONAL,     { CLIColor(CLIColor::Green),   "OPERATIONAL" }},
00073             { uavcan::protocol::NodeStatus::MODE_INITIALIZATION,  { CLIColor(CLIColor::Yellow),  "INITIALIZATION" }},
00074             { uavcan::protocol::NodeStatus::MODE_MAINTENANCE,     { CLIColor(CLIColor::Cyan),    "MAINTENANCE" }},
00075             { uavcan::protocol::NodeStatus::MODE_SOFTWARE_UPDATE, { CLIColor(CLIColor::Magenta), "SOFTWARE_UPDATE" }},
00076             { uavcan::protocol::NodeStatus::MODE_OFFLINE,         { CLIColor(CLIColor::Red),     "OFFLINE" }}
00077         };
00078         try
00079         {
00080             return map.at(mode);
00081         }
00082         catch (std::out_of_range&)
00083         {
00084             return { CLIColor(CLIColor::Red), std::to_string(mode) };
00085         }
00086     }
00087 
00088     void printStatusLine(const uavcan::NodeID nid, const uavcan::NodeStatusMonitor::NodeStatus& status)
00089     {
00090         const auto health_and_color = healthToColoredString(status.health);
00091         const auto mode_and_color   = modeToColoredString(status.mode);
00092 
00093         const int nid_int = nid.get();
00094         const unsigned long uptime = status_registry_[nid_int].uptime_sec;
00095         const unsigned vendor_code = status_registry_[nid_int].vendor_specific_status_code;
00096 
00097         std::printf(" %-3d |", nid_int);
00098         {
00099             CLIColorizer clz(mode_and_color.first);
00100             std::printf(" %-15s ", mode_and_color.second.c_str());
00101         }
00102         std::printf("|");
00103         {
00104             CLIColorizer clz(health_and_color.first);
00105             std::printf(" %-8s ", health_and_color.second.c_str());
00106         }
00107         std::printf("| %-10lu | %04x  %s'%s  %u\n", uptime, vendor_code,
00108                     std::bitset<8>((vendor_code >> 8) & 0xFF).to_string().c_str(),
00109                     std::bitset<8>(vendor_code).to_string().c_str(),
00110                     vendor_code);
00111     }
00112 
00113     void redraw(const uavcan::TimerEvent&)
00114     {
00115         std::printf("\x1b[1J");   // Clear screen from the current cursor position to the beginning
00116         std::printf("\x1b[H");    // Move cursor to the coordinates 1,1
00117         std::printf(" NID | Mode            | Health   | Uptime [s] | Vendor-specific status code\n");
00118         std::printf("-----+-----------------+----------+------------+-hex---bin----------------dec--\n");
00119 
00120         for (unsigned i = 1; i <= uavcan::NodeID::Max; i++)
00121         {
00122             if (isNodeKnown(i))
00123             {
00124                 printStatusLine(i, getNodeStatus(i));
00125             }
00126         }
00127     }
00128 
00129 public:
00130     explicit Monitor(uavcan_linux::NodePtr node)
00131         : uavcan::NodeStatusMonitor(*node)
00132         , timer_(node->makeTimer(uavcan::MonotonicDuration::fromMSec(500),
00133                                  std::bind(&Monitor::redraw, this, std::placeholders::_1)))
00134     { }
00135 };
00136 
00137 
00138 static uavcan_linux::NodePtr initNodeInPassiveMode(const std::vector<std::string>& ifaces, const std::string& node_name)
00139 {
00140     auto node = uavcan_linux::makeNode(ifaces, node_name.c_str(),
00141                                        uavcan::protocol::SoftwareVersion(), uavcan::protocol::HardwareVersion());
00142     node->setModeOperational();
00143     return node;
00144 }
00145 
00146 static void runForever(const uavcan_linux::NodePtr& node)
00147 {
00148     Monitor mon(node);
00149     ENFORCE(0 == mon.start());
00150     while (true)
00151     {
00152         const int res = node->spin(uavcan::MonotonicDuration::getInfinite());
00153         if (res < 0)
00154         {
00155             node->logError("spin", "Error %*", res);
00156         }
00157     }
00158 }
00159 
00160 int main(int argc, const char** argv)
00161 {
00162     try
00163     {
00164         if (argc < 2)
00165         {
00166             std::cerr << "Usage:\n\t" << argv[0] << " <can-iface-name-1> [can-iface-name-N...]" << std::endl;
00167             return 1;
00168         }
00169         std::vector<std::string> iface_names;
00170         for (int i = 1; i < argc; i++)
00171         {
00172             iface_names.emplace_back(argv[i]);
00173         }
00174         uavcan_linux::NodePtr node = initNodeInPassiveMode(iface_names, "org.uavcan.linux_app.node_status_monitor");
00175         runForever(node);
00176         return 0;
00177     }
00178     catch (const std::exception& ex)
00179     {
00180         std::cerr << "Error: " << ex.what() << std::endl;
00181         return 1;
00182     }
00183 }