diff --git a/libuavcan_drivers/linux/CMakeLists.txt b/libuavcan_drivers/linux/CMakeLists.txt index fd8ed5eef1..408fff617c 100644 --- a/libuavcan_drivers/linux/CMakeLists.txt +++ b/libuavcan_drivers/linux/CMakeLists.txt @@ -69,7 +69,11 @@ target_link_libraries(uavcan_status_monitor ${UAVCAN_LIB} rt ${CMAKE_THREAD_LIBS add_executable(uavcan_nodetool apps/uavcan_nodetool.cpp) 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 uavcan_nodetool + uavcan_dynamic_node_id_server RUNTIME DESTINATION bin) \ No newline at end of file diff --git a/libuavcan_drivers/linux/apps/uavcan_dynamic_node_id_server.cpp b/libuavcan_drivers/linux/apps/uavcan_dynamic_node_id_server.cpp new file mode 100644 index 0000000000..5883fb2a82 --- /dev/null +++ b/libuavcan_drivers/linux/apps/uavcan_dynamic_node_id_server.cpp @@ -0,0 +1,546 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "debug.hpp" +// UAVCAN +#include +// UAVCAN Linux drivers +#include +// UAVCAN POSIX drivers +#include +#include + +namespace +{ + +uavcan_linux::NodePtr initNode(const std::vector& ifaces, uavcan::NodeID nid, const std::string& name) +{ + auto node = uavcan_linux::makeNode(ifaces); + + node->setNodeID(nid); + node->setName(name.c_str()); + node->getLogger().setLevel(uavcan::protocol::debug::LogLevel::DEBUG); + + { + const auto machine_id = uavcan_linux::MachineIDReader().read(); + + uavcan::protocol::HardwareVersion hwver; + std::copy(machine_id.begin(), machine_id.end(), hwver.unique_id.begin()); + std::cout << hwver << std::endl; + + node->setHardwareVersion(hwver); + } + + const int start_res = node->start(); + ENFORCE(0 == start_res); + + uavcan::NetworkCompatibilityCheckResult check_result; + ENFORCE(0 == node->checkNetworkCompatibility(check_result)); + if (!check_result.isOk()) + { + throw std::runtime_error("Network conflict with node " + std::to_string(check_result.conflicting_node.get())); + } + + return node; +} + + +class EventTracer : public uavcan_posix::dynamic_node_id_server::FileEventTracer +{ +public: + struct RecentEvent + { + const uavcan::MonotonicDuration time_since_startup; + const uavcan::dynamic_node_id_server::TraceCode code; + const std::int64_t argument; + + RecentEvent(uavcan::MonotonicDuration arg_time_since_startup, + uavcan::dynamic_node_id_server::TraceCode arg_code, + std::int64_t arg_argument) + : time_since_startup(arg_time_since_startup) + , code(arg_code) + , argument(arg_argument) + { } + + uavcan::MakeString<81>::Type toString() const // Heapless return + { + const double ts = time_since_startup.toUSec() / 1e6; + decltype(toString()) out; + out.resize(out.capacity()); + (void)std::snprintf(reinterpret_cast(out.begin()), out.size() - 1U, + "%-11.1f %-28s % -20lld %016llx", + ts, + getEventName(code), + static_cast(argument), + static_cast(argument)); + return out; + } + + static const char* getTableHeader() + { + // Matches the string format above + return "Rel. time Event name Argument (dec) Argument (hex)"; + } + }; + + struct EventStatisticsRecord + { + std::uint64_t count; + uavcan::MonotonicTime last_occurence; + + EventStatisticsRecord() + : count(0) + { } + + void hit(uavcan::MonotonicTime ts) + { + count++; + last_occurence = ts; + } + }; + +private: + struct EnumKeyHash + { + template + std::size_t operator()(T t) const { return static_cast(t); } + }; + + uavcan_linux::SystemClock clock_; + const uavcan::MonotonicTime started_at_ = clock_.getMonotonic(); + const unsigned num_last_events_; + + std::deque last_events_; + std::unordered_map event_counters_; + + bool had_events_ = false; + + virtual void onEvent(uavcan::dynamic_node_id_server::TraceCode code, std::int64_t argument) + { + uavcan_posix::dynamic_node_id_server::FileEventTracer::onEvent(code, argument); + + const auto ts = clock_.getMonotonic(); + const auto time_since_startup = ts - started_at_; + + last_events_.emplace_front(time_since_startup, code, argument); + if (last_events_.size() > num_last_events_) + { + last_events_.pop_back(); + } + + event_counters_[code].hit(ts); + } + +public: + EventTracer(unsigned num_last_events_to_keep) + : num_last_events_(num_last_events_to_keep) + { } + + using uavcan_posix::dynamic_node_id_server::FileEventTracer::init; + + const RecentEvent& getEventByIndex(unsigned index) const { return last_events_.at(index); } + + unsigned getNumEvents() const { return last_events_.size(); } + + const decltype(event_counters_)& getEventCounters() const { return event_counters_; } + + bool hadEvents() + { + if (had_events_) + { + had_events_ = false; + return true; + } + return false; + } +}; + + +::winsize getTerminalSize() +{ + auto w = ::winsize(); + ENFORCE(0 >= ioctl(STDOUT_FILENO, TIOCGWINSZ, &w)); + ENFORCE(w.ws_col > 0 && w.ws_row > 0); + return w; +} + + +std::vector> +collectRelevantEvents(const EventTracer& event_tracer, const unsigned num_events) +{ + // First, creating a vector of pairs (event code, count) + typedef std::pair Pair; + const auto counters = event_tracer.getEventCounters(); + std::vector pairs(counters.size()); + std::copy(counters.begin(), counters.end(), pairs.begin()); + + // Now, sorting the pairs so that the most recent ones are on top of the list + std::sort(pairs.begin(), pairs.end(), [](const Pair& a, const Pair& b) { + return a.second.last_occurence > b.second.last_occurence; + }); + + // Cutting the oldest events away + pairs.resize(std::min(num_events, unsigned(pairs.size()))); + + // Sorting so that the most frequent events are on top of the list + std::sort(pairs.begin(), pairs.end(), [](const Pair& a, const Pair& b) { + return a.second.count > b.second.count; + }); + + return pairs; +} + + +void redraw(const uavcan_linux::NodePtr& node, + const uavcan::MonotonicTime timestamp, + const EventTracer& event_tracer, + const uavcan::dynamic_node_id_server::DistributedServer& server) +{ + /* + * Constants that are permanent for the designed UI layout + */ + constexpr unsigned NumRelevantEvents = 16; + constexpr unsigned NumRowsWithoutEvents = 3; + + /* + * Collecting the data + */ + const unsigned num_rows = getTerminalSize().ws_row; + + const auto relevant_events = collectRelevantEvents(event_tracer, NumRelevantEvents); + + const uavcan::dynamic_node_id_server::distributed::StateReport report(server); + + const auto time_since_last_activity = timestamp - report.last_activity_timestamp; + + /* + * Basic rendering functions + */ + unsigned next_relevant_event_index = 0; + + const auto render_next_event_counter = [&]() + { + const char* event_name = ""; + char event_count_str[10] = { }; + + if (next_relevant_event_index < relevant_events.size()) + { + const auto e = relevant_events[next_relevant_event_index]; + event_name = uavcan::dynamic_node_id_server::IEventTracer::getEventName(e.first); + std::snprintf(event_count_str, sizeof(event_count_str) - 1U, "%llu", + static_cast(e.second.count)); + } + next_relevant_event_index++; + + std::printf(" | %-29s %-9s\n", event_name, event_count_str); + }; + + const auto render_top_str = [&](const char* local_state_name, const char* local_state_value) + { + std::printf("%-20s %-16s", local_state_name, local_state_value); + render_next_event_counter(); + }; + + const auto render_top_int = [&](const char* local_state_name, long long local_state_value) + { + char buf[21]; + std::snprintf(buf, sizeof(buf) - 1U, "%lld", local_state_value); + render_top_str(local_state_name, buf); + }; + + const auto raft_state_to_string = [](uavcan::dynamic_node_id_server::distributed::RaftCore::ServerState s) + { + using uavcan::dynamic_node_id_server::distributed::RaftCore; + switch (s) + { + case RaftCore::ServerStateFollower: return "Follower"; + case RaftCore::ServerStateCandidate: return "Candidate"; + case RaftCore::ServerStateLeader: return "Leader"; + default: return "BADSTATE"; + } + }; + + const auto duration_to_string = [](uavcan::MonotonicDuration dur) + { + uavcan::MakeString<16>::Type str; // This is much faster than std::string + str.appendFormatted("%.1f", dur.toUSec() / 1e6); + return str; + }; + + /* + * Rendering the data to the CLI + */ + std::printf("\x1b\x5b\x48\x1b\x5b\x32\x4a"); // Clear screen and move caret to the upper-left corner + + // Local state and relevant event counters - two columns + std::printf(" Local state | Event counters\n"); + + render_top_int("Node ID", node->getNodeID().get()); + render_top_str("State", raft_state_to_string(report.state)); + render_top_str("Mode", report.is_active ? "Active" : "Passive"); + render_top_int("Last log index", report.last_log_index); + render_top_int("Last log term", report.last_log_term); + render_top_int("Commit index", report.commit_index); + render_top_int("Current term", report.current_term); + render_top_int("Voted for", report.voted_for.get()); + render_top_str("Since activity", duration_to_string(time_since_last_activity).c_str()); + render_top_int("Unknown nodes", report.num_unknown_nodes); + + // Empty line before the next block + std::printf(" "); + render_next_event_counter(); + + // Followers block + std::printf(" Followers "); + render_next_event_counter(); + + const auto render_followers_state = + [&](const char* name, const std::function value_getter) + { + std::printf("%-17s", name); + for (std::uint8_t i = 0; i < 4; i++) + { + if (i < (report.cluster_size - 1)) + { + const auto value = value_getter(i); + if (value >= 0) + { + std::printf("%-5d", value); + } + else + { + std::printf("N/A "); + } + } + else + { + std::printf(" "); + } + } + render_next_event_counter(); + }; + + render_followers_state("Node ID", [&](std::uint8_t i) + { + const auto nid = report.followers[i].node_id; + return nid.isValid() ? nid.get() : -1; + }); + render_followers_state("Next index", [&](std::uint8_t i) { return report.followers[i].next_index; }); + render_followers_state("Match index", [&](std::uint8_t i) { return report.followers[i].match_index; }); + + // Empty line before the next block + std::printf(" "); + render_next_event_counter(); + + assert(next_relevant_event_index == NumRelevantEvents); // Ensuring that all events can be printed + + // Separator + std::printf("--------------------------------------+----------------------------------------\n"); + + // Event log + std::printf("%s", EventTracer::RecentEvent::getTableHeader()); // NO NEWLINE + const int num_events_to_render = static_cast(num_rows) - + static_cast(next_relevant_event_index) - + static_cast(NumRowsWithoutEvents); + for (int i = 0; + i < num_events_to_render && i < static_cast(event_tracer.getNumEvents()); + i++) + { + // NEWLINE IS PREPENDED + std::printf("\n%s", event_tracer.getEventByIndex(i).toString().c_str()); + } + // Note that the last line does not have trailing newline. This allows to use all available rows. + + std::fflush(stdout); +} + + +void runForever(const uavcan_linux::NodePtr& node, + const std::uint8_t cluster_size, + const std::string& event_log_file, + const std::string& persistent_storage_path) +{ + /* + * Event tracer + */ + const unsigned num_last_events_to_keep = 100; + + EventTracer event_tracer(num_last_events_to_keep); + ENFORCE(0 <= event_tracer.init(event_log_file.c_str())); + + /* + * Storage backend + */ + uavcan_posix::dynamic_node_id_server::FileStorageBackend storage_backend; + + ENFORCE(0 <= storage_backend.init(persistent_storage_path.c_str())); + + /* + * Server + */ + uavcan::dynamic_node_id_server::DistributedServer server(*node, storage_backend, event_tracer); + + ENFORCE(0 <= server.init(node->getNodeStatusProvider().getHardwareVersion().unique_id, cluster_size)); + + /* + * Spinning the node + */ + uavcan::MonotonicTime last_redraw_at; + + while (true) + { + const int res = node->spin(uavcan::MonotonicDuration::fromMSec(100)); + if (res < 0) + { + std::cerr << "Spin error: " << res << std::endl; + } + + const auto ts = node->getMonotonicTime(); + + if (event_tracer.hadEvents() || (ts - last_redraw_at).toMSec() > 1000) + { + last_redraw_at = ts; + redraw(node, ts, event_tracer, server); + } + } +} + +struct Options +{ + uavcan::NodeID node_id; + std::vector ifaces; + std::uint8_t cluster_size = 0; + std::string storage_path; +}; + +Options parseOptions(int argc, const char** argv) +{ + const char* const executable_name = *argv++; + argc--; + + const auto enforce = [executable_name](bool condition, const char* error_text) { + if (!condition) + { + std::cerr << error_text << "\n" + << "Usage:\n\t" + << executable_name + << " [can-iface-name-N...] [-c ] -s ]" + << std::endl; + std::exit(1); + } + }; + + enforce(argc >= 3, "Not enough arguments"); + + /* + * Node ID is always at the first position + */ + argc--; + const int node_id = std::stoi(*argv++); + enforce(node_id >= 1 && node_id <= 127, "Invalid node ID"); + + Options out; + out.node_id = uavcan::NodeID(std::uint8_t(node_id)); + + while (argc --> 0) + { + const std::string token(*argv++); + + if (token[0] != '-') + { + out.ifaces.push_back(token); + } + else if (token[1] == 'c') + { + int cluster_size = 0; + if (token.length() > 2) // -c2 + { + cluster_size = std::stoi(token.c_str() + 2); + } + else // -c 2 + { + enforce(argc --> 0, "Expected cluster size"); + cluster_size = std::stoi(*argv++); + } + enforce(cluster_size >= 1 && + cluster_size <= uavcan::dynamic_node_id_server::distributed::ClusterManager::MaxClusterSize, + "Invalid cluster size"); + out.cluster_size = std::uint8_t(cluster_size); + } + else if (token[1] == 's') + { + if (token.length() > 2) // -s/foo/bar + { + out.storage_path = token.c_str() + 2; + } + else // -s /foo/bar + { + enforce(argc --> 0, "Expected storage path"); + out.storage_path = *argv++; + } + } + else + { + enforce(false, "Unexpected argument"); + } + } + + enforce(!out.storage_path.empty(), "Invalid storage path"); + + return out; +} + +} + +int main(int argc, const char** argv) +{ + try + { + const auto options = parseOptions(argc, argv); + + std::cout << "Self node ID: " << int(options.node_id.get()) << "\n" + "Cluster size: " << int(options.cluster_size) << "\n" + "Storage path: " << options.storage_path << "\n" + "Num ifaces: " << options.ifaces.size() << "\n" +#ifdef NDEBUG + "Build mode: Release" +#else + "Build mode: Debug" +#endif + << std::endl; + + /* + * Preparing the storage directory + */ + (void)std::system(("mkdir -p '" + options.storage_path + "' &>/dev/null").c_str()); + + const auto event_log_file = options.storage_path + "/events.log"; + const auto storage_path = options.storage_path + "/storage/"; + + /* + * Starting the node + */ + auto node = initNode(options.ifaces, options.node_id, "org.uavcan.linux_app.dynamic_node_id_server"); + runForever(node, options.cluster_size, event_log_file, storage_path); + return 0; + } + catch (const std::exception& ex) + { + std::cerr << "Error: " << ex.what() << std::endl; + return 1; + } +}