uavcan_dynamic_node_id_server.cpp

00001 /*
00002  * Copyright (C) 2015 Pavel Kirienko <pavel.kirienko@gmail.com>
00003  */
00004 
00005 #include <iostream>
00006 #include <algorithm>
00007 #include <sstream>
00008 #include <iomanip>
00009 #include <string>
00010 #include <cstdlib>
00011 #include <cstdio>
00012 #include <deque>
00013 #include <unordered_map>
00014 #include <sys/types.h>
00015 #include <sys/stat.h>
00016 #include <sys/ioctl.h>
00017 #include <unistd.h>
00018 #include "debug.hpp"
00019 // UAVCAN
00020 #include <uavcan/protocol/dynamic_node_id_server/distributed.hpp>
00021 // UAVCAN Linux drivers
00022 #include <uavcan_linux/uavcan_linux.hpp>
00023 // UAVCAN POSIX drivers
00024 #include <uavcan_posix/dynamic_node_id_server/file_storage_backend.hpp>
00025 #include <uavcan_posix/dynamic_node_id_server/file_event_tracer.hpp>
00026 
00027 namespace
00028 {
00029 
00030 constexpr int MaxNumLastEvents = 30;
00031 constexpr int MinUpdateInterval = 100;
00032 
00033 uavcan_linux::NodePtr initNode(const std::vector<std::string>& ifaces, uavcan::NodeID nid, const std::string& name)
00034 {
00035     const auto app_id = uavcan_linux::makeApplicationID(uavcan_linux::MachineIDReader().read(),  name,  nid.get());
00036 
00037     uavcan::protocol::HardwareVersion hwver;
00038     std::copy(app_id.begin(), app_id.end(), hwver.unique_id.begin());
00039     std::cout << hwver << std::endl;
00040 
00041     auto node = uavcan_linux::makeNode(ifaces, name.c_str(), uavcan::protocol::SoftwareVersion(), hwver, nid);
00042 
00043     node->getLogger().setLevel(uavcan::protocol::debug::LogLevel::DEBUG);
00044     node->setModeOperational();
00045 
00046     return node;
00047 }
00048 
00049 
00050 class EventTracer : public uavcan_posix::dynamic_node_id_server::FileEventTracer
00051 {
00052 public:
00053     struct RecentEvent
00054     {
00055         const uavcan::MonotonicDuration time_since_startup;
00056         const uavcan::UtcTime utc_timestamp;
00057         const uavcan::dynamic_node_id_server::TraceCode code;
00058         const std::int64_t argument;
00059 
00060         RecentEvent(uavcan::MonotonicDuration arg_time_since_startup,
00061                     uavcan::UtcTime arg_utc_timestamp,
00062                     uavcan::dynamic_node_id_server::TraceCode arg_code,
00063                     std::int64_t arg_argument)
00064             : time_since_startup(arg_time_since_startup)
00065             , utc_timestamp(arg_utc_timestamp)
00066             , code(arg_code)
00067             , argument(arg_argument)
00068         { }
00069 
00070         uavcan::MakeString<81>::Type toString() const   // Heapless return
00071         {
00072             char timebuf[12] = { };
00073             {
00074                 const std::time_t rawtime = utc_timestamp.toUSec() * 1e-6;
00075                 const auto tm = std::localtime(&rawtime);
00076                 std::strftime(timebuf, 10, "%H:%M:%S.", tm);
00077                 std::snprintf(&timebuf[9], 3, "%02u", static_cast<unsigned>((utc_timestamp.toMSec() % 1000U) / 10U));
00078             }
00079 
00080             decltype(toString()) out;
00081             out.resize(out.capacity());
00082             // coverity[overflow : FALSE]
00083             (void)std::snprintf(reinterpret_cast<char*>(out.begin()), out.size() - 1U,
00084                                 "%-11s  %-28s %-20lld %016llx",
00085                                 timebuf,
00086                                 getEventName(code),
00087                                 static_cast<long long>(argument),
00088                                 static_cast<long long>(argument));
00089             return out;
00090         }
00091 
00092         static const char* getTableHeader()
00093         {
00094             // Matches the string format above
00095             return "Timestamp    Event name                   Argument (dec)       Argument (hex)";
00096         }
00097     };
00098 
00099     struct EventStatisticsRecord
00100     {
00101         std::uint64_t count;
00102         uavcan::MonotonicTime last_occurence;
00103 
00104         EventStatisticsRecord()
00105             : count(0)
00106         { }
00107 
00108         void hit(uavcan::MonotonicTime ts)
00109         {
00110             count++;
00111             last_occurence = ts;
00112         }
00113     };
00114 
00115 private:
00116     struct EnumKeyHash
00117     {
00118         template <typename T>
00119         std::size_t operator()(T t) const { return static_cast<std::size_t>(t); }
00120     };
00121 
00122     uavcan_linux::SystemClock clock_;
00123     const uavcan::MonotonicTime started_at_ = clock_.getMonotonic();
00124     const unsigned num_last_events_;
00125 
00126     std::deque<RecentEvent> last_events_;
00127     std::unordered_map<uavcan::dynamic_node_id_server::TraceCode, EventStatisticsRecord, EnumKeyHash> event_counters_;
00128 
00129     bool had_events_ = false;
00130 
00131     void onEvent(uavcan::dynamic_node_id_server::TraceCode code, std::int64_t argument) override
00132     {
00133         uavcan_posix::dynamic_node_id_server::FileEventTracer::onEvent(code, argument);
00134 
00135         had_events_ = true;
00136 
00137         const auto ts_m = clock_.getMonotonic();
00138         const auto ts_utc = clock_.getUtc();
00139         const auto time_since_startup = ts_m - started_at_;
00140 
00141         last_events_.emplace_front(time_since_startup, ts_utc, code, argument);
00142         if (last_events_.size() > num_last_events_)
00143         {
00144             last_events_.pop_back();
00145         }
00146 
00147         event_counters_[code].hit(ts_m);
00148     }
00149 
00150 public:
00151     EventTracer(unsigned num_last_events_to_keep)
00152         : num_last_events_(num_last_events_to_keep)
00153     { }
00154 
00155     using uavcan_posix::dynamic_node_id_server::FileEventTracer::init;
00156 
00157     const RecentEvent& getEventByIndex(unsigned index) const { return last_events_.at(index); }
00158 
00159     unsigned getNumEvents() const { return last_events_.size(); }
00160 
00161     const decltype(event_counters_)& getEventCounters() const { return event_counters_; }
00162 
00163     bool hadEvents()
00164     {
00165         if (had_events_)
00166         {
00167             had_events_ = false;
00168             return true;
00169         }
00170         return false;
00171     }
00172 };
00173 
00174 
00175 ::winsize getTerminalSize()
00176 {
00177     auto w = ::winsize();
00178     ENFORCE(0 >= ioctl(STDOUT_FILENO, TIOCGWINSZ, &w));
00179     ENFORCE(w.ws_col > 0 && w.ws_row > 0);
00180     return w;
00181 }
00182 
00183 
00184 std::vector<std::pair<uavcan::dynamic_node_id_server::TraceCode, EventTracer::EventStatisticsRecord>>
00185 collectRelevantEvents(const EventTracer& event_tracer, const unsigned num_events)
00186 {
00187     // First, creating a vector of pairs (event code, count)
00188     typedef std::pair<uavcan::dynamic_node_id_server::TraceCode, EventTracer::EventStatisticsRecord> Pair;
00189     const auto counters = event_tracer.getEventCounters();
00190     std::vector<Pair> pairs(counters.size());
00191     std::copy(counters.begin(), counters.end(), pairs.begin());
00192 
00193     // Now, sorting the pairs so that the most recent ones are on top of the list
00194     std::sort(pairs.begin(), pairs.end(), [](const Pair& a, const Pair& b) {
00195         return a.second.last_occurence > b.second.last_occurence;
00196     });
00197 
00198     // Cutting the oldest events away
00199     pairs.resize(std::min(num_events, unsigned(pairs.size())));
00200 
00201     // Sorting so that the most frequent events are on top of the list
00202     std::stable_sort(pairs.begin(), pairs.end(), [](const Pair& a, const Pair& b) {
00203         return a.second.count > b.second.count;
00204     });
00205 
00206     return pairs;
00207 }
00208 
00209 enum class CLIColor : unsigned
00210 {
00211     Red     = 31,
00212     Green   = 32,
00213     Yellow  = 33,
00214     Blue    = 34,
00215     Magenta = 35,
00216     Cyan    = 36,
00217     White   = 37,
00218     Default = 39
00219 };
00220 
00221 CLIColor getColorHash(unsigned value) { return CLIColor(31 + value % 7); }
00222 
00223 class CLIColorizer
00224 {
00225     const CLIColor color_;
00226 public:
00227     explicit CLIColorizer(CLIColor c) : color_(c)
00228     {
00229         std::printf("\033[%um", static_cast<unsigned>(color_));
00230     }
00231 
00232     ~CLIColorizer()
00233     {
00234         std::printf("\033[%um", static_cast<unsigned>(CLIColor::Default));
00235     }
00236 };
00237 
00238 
00239 void redraw(const uavcan_linux::NodePtr& node,
00240             const uavcan::MonotonicTime timestamp,
00241             const EventTracer& event_tracer,
00242             const uavcan::dynamic_node_id_server::DistributedServer& server)
00243 {
00244     using uavcan::dynamic_node_id_server::distributed::RaftCore;
00245 
00246     /*
00247      * Constants that are permanent for the designed UI layout
00248      */
00249     constexpr unsigned NumRelevantEvents = 17;
00250     constexpr unsigned NumRowsWithoutEvents = 3;
00251 
00252     /*
00253      * Collecting the data
00254      */
00255     const unsigned num_rows = getTerminalSize().ws_row;
00256 
00257     const auto relevant_events = collectRelevantEvents(event_tracer, NumRelevantEvents);
00258 
00259     const uavcan::dynamic_node_id_server::distributed::StateReport report(server);
00260 
00261     const auto time_since_last_activity = timestamp - report.last_activity_timestamp;
00262 
00263     /*
00264      * Basic rendering functions
00265      */
00266     unsigned next_relevant_event_index = 0;
00267 
00268     const auto render_next_event_counter = [&]()
00269     {
00270         const char* event_name = "";
00271         char event_count_str[10] = { };
00272         CLIColor event_color = CLIColor::Default;
00273 
00274         if (next_relevant_event_index < relevant_events.size())
00275         {
00276             const auto e = relevant_events[next_relevant_event_index];
00277             event_name = uavcan::dynamic_node_id_server::IEventTracer::getEventName(e.first);
00278             std::snprintf(event_count_str, sizeof(event_count_str) - 1U, "%llu",
00279                           static_cast<unsigned long long>(e.second.count));
00280             event_color = getColorHash(static_cast<unsigned>(e.first));
00281         }
00282         next_relevant_event_index++;
00283 
00284         std::printf(" | ");
00285         CLIColorizer izer(event_color);
00286         std::printf("%-29s %-9s\n", event_name, event_count_str);
00287     };
00288 
00289     const auto render_top_str = [&](const char* local_state_name, const char* local_state_value, CLIColor color)
00290     {
00291         {
00292             CLIColorizer izer(color);
00293             std::printf("%-20s %-16s", local_state_name, local_state_value);
00294         }
00295         render_next_event_counter();
00296     };
00297 
00298     const auto render_top_int = [&](const char* local_state_name, long long local_state_value, CLIColor color)
00299     {
00300         char buf[21];
00301         std::snprintf(buf, sizeof(buf) - 1U, "%lld", local_state_value);
00302         render_top_str(local_state_name, buf, color);
00303     };
00304 
00305     const auto raft_state_to_string = [](uavcan::dynamic_node_id_server::distributed::RaftCore::ServerState s)
00306     {
00307         switch (s)
00308         {
00309         case RaftCore::ServerStateFollower:  return "Follower";
00310         case RaftCore::ServerStateCandidate: return "Candidate";
00311         case RaftCore::ServerStateLeader:    return "Leader";
00312         default:                             return "BADSTATE";
00313         }
00314     };
00315 
00316     const auto duration_to_string = [](uavcan::MonotonicDuration dur)
00317     {
00318         uavcan::MakeString<16>::Type str;                       // This is much faster than std::string
00319         str.appendFormatted("%.1f", dur.toUSec() / 1e6);
00320         return str;
00321     };
00322 
00323     const auto colorize_if = [](bool condition, CLIColor color)
00324     {
00325         return condition ? color : CLIColor::Default;
00326     };
00327 
00328     /*
00329      * Rendering the data to the CLI
00330      */
00331     std::printf("\x1b[1J"); // Clear screen from the current cursor position to the beginning
00332     std::printf("\x1b[H");  // Move cursor to the coordinates 1,1
00333 
00334     // Local state and relevant event counters - two columns
00335     std::printf("        Local state                   |         Event counters\n");
00336 
00337     render_top_int("Node ID",
00338                    node->getNodeID().get(),
00339                    CLIColor::Default);
00340 
00341     render_top_str("State",
00342                    raft_state_to_string(report.state),
00343                    (report.state == RaftCore::ServerStateCandidate) ? CLIColor::Magenta :
00344                        (report.state == RaftCore::ServerStateLeader) ? CLIColor::Green :
00345                            CLIColor::Default);
00346 
00347     render_top_int("Last log index",
00348                    report.last_log_index,
00349                    CLIColor::Default);
00350 
00351     render_top_int("Commit index",
00352                    report.commit_index,
00353                    colorize_if(report.commit_index != report.last_log_index, CLIColor::Magenta));
00354 
00355     render_top_int("Last log term",
00356                    report.last_log_term,
00357                    CLIColor::Default);
00358 
00359     render_top_int("Current term",
00360                    report.current_term,
00361                    CLIColor::Default);
00362 
00363     render_top_int("Voted for",
00364                    report.voted_for.get(),
00365                    CLIColor::Default);
00366 
00367     render_top_str("Since activity",
00368                    duration_to_string(time_since_last_activity).c_str(),
00369                    CLIColor::Default);
00370 
00371     render_top_str("Random timeout",
00372                    duration_to_string(report.randomized_timeout).c_str(),
00373                    CLIColor::Default);
00374 
00375     render_top_int("Unknown nodes",
00376                    report.num_unknown_nodes,
00377                    colorize_if(report.num_unknown_nodes != 0, CLIColor::Magenta));
00378 
00379     render_top_int("Node failures",
00380                    node->getInternalFailureCount(),
00381                    colorize_if(node->getInternalFailureCount() != 0, CLIColor::Magenta));
00382 
00383     const bool all_allocated = server.guessIfAllDynamicNodesAreAllocated();
00384     render_top_str("All allocated",
00385                    all_allocated ? "Yes": "No",
00386                    colorize_if(!all_allocated, CLIColor::Magenta));
00387 
00388     // Empty line before the next block
00389     std::printf("                                     ");
00390     render_next_event_counter();
00391 
00392     // Followers block
00393     std::printf("        Followers                    ");
00394     render_next_event_counter();
00395 
00396     const auto render_followers_state = [&](const char* name,
00397                                             const std::function<int (std::uint8_t)> value_getter,
00398                                             const std::function<CLIColor (std::uint8_t)> color_getter)
00399     {
00400         std::printf("%-17s", name);
00401         for (std::uint8_t i = 0; i < 4; i++)
00402         {
00403             if (i < (report.cluster_size - 1))
00404             {
00405                 CLIColorizer colorizer(color_getter(i));
00406                 const auto value = value_getter(i);
00407                 if (value >= 0)
00408                 {
00409                     std::printf("%-5d", value);
00410                 }
00411                 else
00412                 {
00413                     std::printf("N/A  ");
00414                 }
00415             }
00416             else
00417             {
00418                 std::printf("     ");
00419             }
00420         }
00421         render_next_event_counter();
00422     };
00423 
00424     const auto follower_color_getter = [&](std::uint8_t i)
00425     {
00426         if (report.state != RaftCore::ServerStateLeader)              { return CLIColor::Default; }
00427         if (!report.followers[i].node_id.isValid())                   { return CLIColor::Red; }
00428         if (report.followers[i].match_index != report.last_log_index ||
00429             report.followers[i].next_index <= report.last_log_index)
00430         {
00431             return CLIColor::Magenta;
00432         }
00433         return CLIColor::Default;
00434     };
00435 
00436     render_followers_state("Node ID", [&](std::uint8_t i)
00437                            {
00438                                const auto nid = report.followers[i].node_id;
00439                                return nid.isValid() ? nid.get() : -1;
00440                            },
00441                            follower_color_getter);
00442 
00443     render_followers_state("Next index",
00444                            [&](std::uint8_t i) { return report.followers[i].next_index; },
00445                            follower_color_getter);
00446 
00447     render_followers_state("Match index",
00448                            [&](std::uint8_t i) { return report.followers[i].match_index; },
00449                            follower_color_getter);
00450 
00451     assert(next_relevant_event_index == NumRelevantEvents);     // Ensuring that all events can be printed
00452 
00453     // Separator
00454     std::printf("--------------------------------------+----------------------------------------\n");
00455 
00456     // Event log
00457     std::printf("%s\n", EventTracer::RecentEvent::getTableHeader());
00458     const int num_events_to_render = static_cast<int>(num_rows) -
00459                                      static_cast<int>(next_relevant_event_index) -
00460                                      static_cast<int>(NumRowsWithoutEvents) -
00461                                      1; // This allows to keep the last line empty for stdout or UAVCAN_TRACE()
00462     for (int i = 0;
00463          i < num_events_to_render && i < static_cast<int>(event_tracer.getNumEvents());
00464          i++)
00465     {
00466         const auto e = event_tracer.getEventByIndex(i);
00467         CLIColorizer colorizer(getColorHash(static_cast<unsigned>(e.code)));
00468         std::printf("%s\n", e.toString().c_str());
00469     }
00470 
00471     std::fflush(stdout);
00472 }
00473 
00474 
00475 void runForever(const uavcan_linux::NodePtr& node,
00476                 const std::uint8_t cluster_size,
00477                 const std::string& event_log_file,
00478                 const std::string& persistent_storage_path)
00479 {
00480     /*
00481      * Event tracer
00482      */
00483     EventTracer event_tracer(MaxNumLastEvents);
00484     ENFORCE(0 <= event_tracer.init(event_log_file.c_str()));
00485 
00486     /*
00487      * Storage backend
00488      */
00489     uavcan_posix::dynamic_node_id_server::FileStorageBackend storage_backend;
00490     ENFORCE(0 <= storage_backend.init(persistent_storage_path.c_str()));
00491 
00492     /*
00493      * Server
00494      */
00495     uavcan::dynamic_node_id_server::DistributedServer server(*node, storage_backend, event_tracer);
00496 
00497     const int server_init_res = server.init(node->getNodeStatusProvider().getHardwareVersion().unique_id, cluster_size);
00498     if (server_init_res < 0)
00499     {
00500         throw std::runtime_error("Failed to start the server; error " + std::to_string(server_init_res));
00501     }
00502 
00503     /*
00504      * Preparing the CLI
00505      */
00506     std::printf("\x1b[2J"); // Clear entire screen; this will preserve initialization output in the scrollback
00507 
00508     /*
00509      * Spinning the node
00510      */
00511     uavcan::MonotonicTime last_redraw_at;
00512 
00513     while (true)
00514     {
00515         const int res = node->spin(uavcan::MonotonicDuration::fromMSec(MinUpdateInterval));
00516         if (res < 0)
00517         {
00518             std::cerr << "Spin error: " << res << std::endl;
00519         }
00520 
00521         const auto ts = node->getMonotonicTime();
00522 
00523         if (event_tracer.hadEvents() || (ts - last_redraw_at).toMSec() > 1000)
00524         {
00525             last_redraw_at = ts;
00526             redraw(node, ts, event_tracer, server);
00527         }
00528     }
00529 }
00530 
00531 struct Options
00532 {
00533     uavcan::NodeID node_id;
00534     std::vector<std::string> ifaces;
00535     std::uint8_t cluster_size = 0;
00536     std::string storage_path;
00537 };
00538 
00539 Options parseOptions(int argc, const char** argv)
00540 {
00541     const char* const executable_name = *argv++;
00542     argc--;
00543 
00544     const auto enforce = [executable_name](bool condition, const char* error_text) {
00545         if (!condition)
00546         {
00547             std::cerr << error_text << "\n"
00548                       << "Usage:\n\t"
00549                       << executable_name
00550                       << " <node-id> <can-iface-name-1> [can-iface-name-N...] [-c <cluster-size>] -s <storage-path>"
00551                       << std::endl;
00552             std::exit(1);
00553         }
00554     };
00555 
00556     enforce(argc >= 3, "Not enough arguments");
00557 
00558     /*
00559      * Node ID is always at the first position
00560      */
00561     argc--;
00562     const int node_id = std::stoi(*argv++);
00563     enforce(node_id >= 1 && node_id <= 127, "Invalid node ID");
00564 
00565     Options out;
00566     out.node_id = uavcan::NodeID(std::uint8_t(node_id));
00567 
00568     while (argc --> 0)
00569     {
00570         const std::string token(*argv++);
00571 
00572         if (token[0] != '-')
00573         {
00574             out.ifaces.push_back(token);
00575         }
00576         else if (token[1] == 'c')
00577         {
00578             int cluster_size = 0;
00579             if (token.length() > 2)     // -c2
00580             {
00581                 cluster_size = std::stoi(token.c_str() + 2);
00582             }
00583             else                        // -c 2
00584             {
00585                 enforce(argc --> 0, "Expected cluster size");
00586                 cluster_size = std::stoi(*argv++);
00587             }
00588             enforce(cluster_size >= 1 &&
00589                     cluster_size <= uavcan::dynamic_node_id_server::distributed::ClusterManager::MaxClusterSize,
00590                     "Invalid cluster size");
00591             out.cluster_size = std::uint8_t(cluster_size);
00592         }
00593         else if (token[1] == 's')
00594         {
00595             if (token.length() > 2)     // -s/foo/bar
00596             {
00597                 out.storage_path = token.c_str() + 2;
00598             }
00599             else                        // -s /foo/bar
00600             {
00601                 enforce(argc --> 0, "Expected storage path");
00602                 out.storage_path = *argv++;
00603             }
00604         }
00605         else
00606         {
00607             enforce(false, "Unexpected argument");
00608         }
00609     }
00610 
00611     enforce(!out.storage_path.empty(), "Invalid storage path");
00612 
00613     return out;
00614 }
00615 
00616 }
00617 
00618 int main(int argc, const char** argv)
00619 {
00620     try
00621     {
00622         std::srand(std::time(nullptr));
00623 
00624         if (isatty(STDOUT_FILENO) != 1)
00625         {
00626             std::cerr << "This application cannot run if stdout is not associated with a terminal" << std::endl;
00627             std::exit(1);
00628         }
00629 
00630         auto options = parseOptions(argc, argv);
00631 
00632         std::cout << "Self node ID: " << int(options.node_id.get()) << "\n"
00633                      "Cluster size: " << int(options.cluster_size) << "\n"
00634                      "Storage path: " << options.storage_path << "\n"
00635                      "Num ifaces:   " << options.ifaces.size() << "\n"
00636 #ifdef NDEBUG
00637                      "Build mode:   Release"
00638 #else
00639                      "Build mode:   Debug"
00640 #endif
00641                      << std::endl;
00642 
00643         /*
00644          * Preparing the storage directory
00645          */
00646         options.storage_path += "/node_" + std::to_string(options.node_id.get());
00647 
00648         int system_res = std::system(("mkdir -p '" + options.storage_path + "' &>/dev/null").c_str());
00649         (void)system_res;
00650 
00651         const auto event_log_file = options.storage_path + "/events.log";
00652         const auto storage_path   = options.storage_path + "/storage/";
00653 
00654         /*
00655          * Starting the node
00656          */
00657         auto node = initNode(options.ifaces, options.node_id, "org.uavcan.linux_app.dynamic_node_id_server");
00658         runForever(node, options.cluster_size, event_log_file, storage_path);
00659         return 0;
00660     }
00661     catch (const std::exception& ex)
00662     {
00663         std::cerr << "Error: " << ex.what() << std::endl;
00664         return 1;
00665     }
00666 }