Rearranged Linux tests

libuavcan_drivers/linux/apps/debug.hpp

@@ -0,0 +1,14 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#pragma once
+
+#include <stdexcept>
+
+#ifndef STRINGIZE
+#  define STRINGIZE2(x)   #x
+#  define STRINGIZE(x)    STRINGIZE2(x)
+#endif
+#define ENFORCE(x) if (!(x)) { throw std::runtime_error(__FILE__ ":" STRINGIZE(__LINE__) ": " #x); }
+

libuavcan_drivers/linux/apps/test_clock.cpp

@@ -0,0 +1,70 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include <iostream>
+#include <cerrno>
+#include <chrono>
+#include <uavcan_linux/uavcan_linux.hpp>
+
+static std::string systime2str(const std::chrono::system_clock::time_point& tp)
+{
+    const auto tt = std::chrono::system_clock::to_time_t(tp);
+    return std::ctime(&tt);
+}
+
+int main()
+{
+    uavcan_linux::SystemClock clock;
+
+    /*
+     * Auto-detected clock adjustment mode
+     */
+    std::cout << "Clock adjustment mode: ";
+    switch (clock.getAdjustmentMode())
+    {
+    case uavcan_linux::ClockAdjustmentMode::SystemWide:
+    {
+        std::cout << "SystemWide";
+        break;
+    }
+    case uavcan_linux::ClockAdjustmentMode::PerDriverPrivate:
+    {
+        std::cout << "PerDriverPrivate";
+        break;
+    }
+    default:
+    {
+        std::abort();
+        break;
+    }
+    }
+    std::cout << std::endl;
+
+    /*
+     * Test adjustment
+     */
+    double sec = 0;
+    std::cout << "Enter system time adjustment in seconds (fractions allowed): " << std::endl;
+    std::cin >> sec;
+
+    const auto before = std::chrono::system_clock::now();
+    try
+    {
+        clock.adjustUtc(uavcan::UtcDuration::fromUSec(sec * 1e6));
+    }
+    catch (const uavcan_linux::Exception& ex)
+    {
+        std::cout << ex.what() << std::endl;
+        std::cout << strerror(ex.getErrno()) << std::endl;
+        return 1;
+    }
+    const auto after = std::chrono::system_clock::now();
+
+    std::cout << "Time before: " << systime2str(before) << "\n"
+              << "Time after:  " << systime2str(after) << "\n"
+              << "Millisecond diff (after - before): "
+              << std::chrono::duration_cast<std::chrono::milliseconds>(after - before).count() << std::endl;
+
+    return 0;
+}

libuavcan_drivers/linux/apps/test_node.cpp

@@ -0,0 +1,120 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include <iostream>
+#include <uavcan_linux/uavcan_linux.hpp>
+#include <uavcan/protocol/node_status_monitor.hpp>
+#include "debug.hpp"
+
+static uavcan_linux::NodePtr initNode(const std::vector<std::string>& ifaces, uavcan::NodeID nid,
+                                      const std::string& name)
+{
+    auto node = uavcan_linux::makeNode(ifaces);
+
+    /*
+     * Configuring the node.
+     */
+    node->setNodeID(nid);
+    node->setName(name.c_str());
+
+    node->getLogger().setLevel(uavcan::protocol::debug::LogLevel::DEBUG);
+
+    /*
+     * Starting the node.
+     */
+    std::cout << "Starting the node..." << std::endl;
+    const int start_res = node->start();
+    std::cout << "Start returned: " << start_res << std::endl;
+    ENFORCE(0 == start_res);
+
+    /*
+     * Checking if our node conflicts with other nodes. This may take a few seconds.
+     */
+    uavcan::NetworkCompatibilityCheckResult init_result;
+    ENFORCE(0 == node->checkNetworkCompatibility(init_result));
+    if (!init_result.isOk())
+    {
+        throw std::runtime_error("Network conflict with node " + std::to_string(init_result.conflicting_node.get()));
+    }
+
+    std::cout << "Node started successfully" << std::endl;
+
+    /*
+     * Say Hi to the world.
+     */
+    node->setStatusOk();
+    node->logInfo("init", "Hello world! I'm [%*], NID %*",
+                  node->getNodeStatusProvider().getName().c_str(), int(node->getNodeID().get()));
+    return node;
+}
+
+static void runForever(const uavcan_linux::NodePtr& node)
+{
+    /*
+     * Subscribing to the UAVCAN logging topic
+     */
+    auto log_handler = [](const uavcan::ReceivedDataStructure<uavcan::protocol::debug::LogMessage>& msg)
+    {
+        std::cout << msg << std::endl;
+    };
+    auto log_sub = node->makeSubscriber<uavcan::protocol::debug::LogMessage>(log_handler);
+
+    /*
+     * Printing when other nodes enter the network or change status
+     */
+    struct NodeStatusMonitor : public uavcan::NodeStatusMonitor
+    {
+        explicit NodeStatusMonitor(uavcan::INode& node) : uavcan::NodeStatusMonitor(node) { }
+
+        virtual void handleNodeStatusChange(const NodeStatusChangeEvent& event)
+        {
+            std::cout << "Remote node NID " << int(event.node_id.get()) << " changed status: "
+                      << int(event.old_status.status_code) << " --> "
+                      << int(event.status.status_code) << std::endl;
+        }
+    };
+
+    NodeStatusMonitor nsm(*node);
+    ENFORCE(0 == nsm.start());
+
+    /*
+     * Adding a stupid timer that does nothing once a minute
+     */
+    auto do_nothing_once_a_minute = [&node](const uavcan::TimerEvent&)
+    {
+        node->logInfo("timer", "Another minute passed...");
+    };
+    auto timer = node->makeTimer(uavcan::MonotonicDuration::fromMSec(60000), do_nothing_once_a_minute);
+
+    /*
+     * Spinning forever
+     */
+    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)
+{
+    if (argc < 3)
+    {
+        std::cout << "Usage:\n\t" << argv[0] << " <node-id> <can-iface-name-1> [can-iface-name-N...]" << std::endl;
+        return 1;
+    }
+    const int self_node_id = std::stoi(argv[1]);
+    std::vector<std::string> iface_names;
+    for (int i = 2; i < argc; i++)
+    {
+        iface_names.emplace_back(argv[i]);
+    }
+    uavcan_linux::NodePtr node = initNode(iface_names, self_node_id, "org.uavcan.linux_test_node");
+    std::cout << "Node initialized successfully" << std::endl;
+    runForever(node);
+    return 0;
+}

libuavcan_drivers/linux/apps/test_socket.cpp

@@ -0,0 +1,354 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include <iostream>
+#include <vector>
+#include <cerrno>
+#include <uavcan_linux/uavcan_linux.hpp>
+#include "debug.hpp"
+
+static uavcan::CanFrame makeFrame(std::uint32_t id, const std::string& data)
+{
+    return uavcan::CanFrame(id, reinterpret_cast<const std::uint8_t*>(data.c_str()), data.length());
+}
+
+static uavcan::MonotonicTime tsMonoOffsetMs(std::int64_t ms)
+{
+    return uavcan_linux::SystemClock().getMonotonic() + uavcan::MonotonicDuration::fromMSec(ms);
+}
+
+static void testNonexistentIface()
+{
+    const int sock1 = uavcan_linux::SocketCanIface::openSocket("noif9");
+    ENFORCE(sock1 < 0);
+    const int sock2 = uavcan_linux::SocketCanIface::openSocket("verylongifacenameverylongifacenameverylongifacename");
+    ENFORCE(sock2 < 0);
+}
+
+static void testSocketRxTx(const std::string& iface_name)
+{
+    const int sock1 = uavcan_linux::SocketCanIface::openSocket(iface_name);
+    const int sock2 = uavcan_linux::SocketCanIface::openSocket(iface_name);
+    ENFORCE(sock1 >= 0 && sock2 >= 0);
+
+    /*
+     * Clocks will have some offset from the true system time
+     * SocketCAN driver must handle this correctly
+     */
+    uavcan_linux::SystemClock clock_impl(uavcan_linux::ClockAdjustmentMode::PerDriverPrivate);
+    clock_impl.adjustUtc(uavcan::UtcDuration::fromMSec(100000));
+    const uavcan_linux::SystemClock& clock = clock_impl;
+
+    uavcan_linux::SocketCanIface if1(clock, sock1);
+    uavcan_linux::SocketCanIface if2(clock, sock2);
+
+    /*
+     * Sending two frames, one of which must be returned back
+     */
+    ENFORCE(1 == if1.send(makeFrame(123, "if1-1"), tsMonoOffsetMs(100), 0));
+    ENFORCE(1 == if1.send(makeFrame(456, "if1-2"), tsMonoOffsetMs(100), uavcan::CanIOFlagLoopback));
+    if1.poll(true, true);
+    if1.poll(true, true);
+    ENFORCE(0 == if1.getErrorCount());
+    ENFORCE(!if1.hasPendingTx());
+    ENFORCE(if1.hasReadyRx());       // Second loopback
+
+    /*
+     * Second iface, same thing
+     */
+    ENFORCE(1 == if2.send(makeFrame(321, "if2-1"), tsMonoOffsetMs(100), 0));
+    ENFORCE(1 == if2.send(makeFrame(654, "if2-2"), tsMonoOffsetMs(100), uavcan::CanIOFlagLoopback));
+    ENFORCE(1 == if2.send(makeFrame(1, "discard"), tsMonoOffsetMs(-1), uavcan::CanIOFlagLoopback));  // Will timeout
+    if2.poll(true, true);
+    if2.poll(true, true);
+    ENFORCE(1 == if2.getErrorCount());  // One timed out
+    ENFORCE(!if2.hasPendingTx());
+    ENFORCE(if2.hasReadyRx());
+
+    /*
+     * No-op
+     */
+    if1.poll(true, true);
+    if2.poll(true, true);
+
+    uavcan::CanFrame frame;
+    uavcan::MonotonicTime ts_mono;
+    uavcan::UtcTime ts_utc;
+    uavcan::CanIOFlags flags = 0;
+
+    /*
+     * Read first
+     */
+    ENFORCE(1 == if1.receive(frame, ts_mono, ts_utc, flags));
+    ENFORCE(frame == makeFrame(456, "if1-2"));
+    ENFORCE(flags == uavcan::CanIOFlagLoopback);
+    ENFORCE((clock.getMonotonic() - ts_mono).getAbs().toMSec() < 10);
+    ENFORCE((clock.getUtc() - ts_utc).getAbs().toMSec() < 10);
+
+    ENFORCE(1 == if1.receive(frame, ts_mono, ts_utc, flags));
+    ENFORCE(frame == makeFrame(321, "if2-1"));
+    ENFORCE(flags == 0);
+    ENFORCE(!ts_mono.isZero());
+    ENFORCE(!ts_utc.isZero());
+    ENFORCE((clock.getMonotonic() - ts_mono).getAbs().toMSec() < 10);
+    ENFORCE((clock.getUtc() - ts_utc).getAbs().toMSec() < 10);
+
+    ENFORCE(1 == if1.receive(frame, ts_mono, ts_utc, flags));
+    ENFORCE(frame == makeFrame(654, "if2-2"));
+    ENFORCE(flags == 0);
+    ENFORCE((clock.getMonotonic() - ts_mono).getAbs().toMSec() < 10);
+    ENFORCE((clock.getUtc() - ts_utc).getAbs().toMSec() < 10);
+
+    ENFORCE(0 == if1.receive(frame, ts_mono, ts_utc, flags));
+    ENFORCE(!if1.hasPendingTx());
+    ENFORCE(!if1.hasReadyRx());
+
+    /*
+     * Read second
+     */
+    ENFORCE(1 == if2.receive(frame, ts_mono, ts_utc, flags));
+    ENFORCE(frame == makeFrame(123, "if1-1"));
+    ENFORCE(flags == 0);
+    ENFORCE((clock.getMonotonic() - ts_mono).getAbs().toMSec() < 10);
+    ENFORCE((clock.getUtc() - ts_utc).getAbs().toMSec() < 10);
+
+    ENFORCE(1 == if2.receive(frame, ts_mono, ts_utc, flags));
+    ENFORCE(frame == makeFrame(456, "if1-2"));
+    ENFORCE(flags == 0);
+    ENFORCE((clock.getMonotonic() - ts_mono).getAbs().toMSec() < 10);
+    ENFORCE((clock.getUtc() - ts_utc).getAbs().toMSec() < 10);
+
+    ENFORCE(1 == if2.receive(frame, ts_mono, ts_utc, flags));
+    ENFORCE(frame == makeFrame(654, "if2-2"));
+    ENFORCE(flags == uavcan::CanIOFlagLoopback);
+    ENFORCE((clock.getMonotonic() - ts_mono).getAbs().toMSec() < 10);
+    ENFORCE((clock.getUtc() - ts_utc).getAbs().toMSec() < 10);
+
+    ENFORCE(0 == if2.receive(frame, ts_mono, ts_utc, flags));
+    ENFORCE(!if2.hasPendingTx());
+    ENFORCE(!if2.hasReadyRx());
+}
+
+static void testSocketFilters(const std::string& iface_name)
+{
+    using uavcan::CanFrame;
+
+    const int sock1 = uavcan_linux::SocketCanIface::openSocket(iface_name);
+    const int sock2 = uavcan_linux::SocketCanIface::openSocket(iface_name);
+    ENFORCE(sock1 >= 0 && sock2 >= 0);
+
+    /*
+     * Clocks will have some offset from the true system time
+     * SocketCAN driver must handle this correctly
+     */
+    uavcan_linux::SystemClock clock_impl(uavcan_linux::ClockAdjustmentMode::PerDriverPrivate);
+    clock_impl.adjustUtc(uavcan::UtcDuration::fromMSec(-1000));
+    const uavcan_linux::SystemClock& clock = clock_impl;
+
+    uavcan_linux::SocketCanIface if1(clock, sock1);
+    uavcan_linux::SocketCanIface if2(clock, sock2);
+
+    /*
+     * Configuring filters
+     */
+    uavcan::CanFilterConfig fcs[3];
+    // STD/EXT 123
+    fcs[0].id = 123;
+    fcs[0].mask = CanFrame::MaskExtID;
+    // Only EXT 456789
+    fcs[1].id = 456789 | CanFrame::FlagEFF;
+    fcs[1].mask = CanFrame::MaskExtID | CanFrame::FlagEFF;
+    // Only STD 0
+    fcs[2].id = 0;
+    fcs[2].mask = CanFrame::MaskExtID | CanFrame::FlagEFF;
+
+    ENFORCE(0 == if2.configureFilters(fcs, 3));
+
+    /*
+     * Sending data from 1 to 2, making sure only filtered data will be accepted
+     */
+    const auto EFF = CanFrame::FlagEFF;
+    ENFORCE(1 == if1.send(makeFrame(123,          "1"), tsMonoOffsetMs(100), 0)); // Accept 0
+    ENFORCE(1 == if1.send(makeFrame(123 | EFF,    "2"), tsMonoOffsetMs(100), 0)); // Accept 0
+    ENFORCE(1 == if1.send(makeFrame(456,          "3"), tsMonoOffsetMs(100), 0)); // Drop
+    ENFORCE(1 == if1.send(makeFrame(456789,       "4"), tsMonoOffsetMs(100), 0)); // Drop
+    ENFORCE(1 == if1.send(makeFrame(456789 | EFF, "5"), tsMonoOffsetMs(100), 0)); // Accept 1
+    ENFORCE(1 == if1.send(makeFrame(0,            "6"), tsMonoOffsetMs(100), 0)); // Accept 2
+    ENFORCE(1 == if1.send(makeFrame(EFF,          "7"), tsMonoOffsetMs(100), 0)); // Drop
+
+    for (int i = 0; i < 7; i++)
+    {
+        if1.poll(true, true);
+        if2.poll(true, false);
+    }
+    ENFORCE(!if1.hasPendingTx());
+    ENFORCE(!if1.hasReadyRx());
+    ENFORCE(0 == if1.getErrorCount());
+    ENFORCE(if2.hasReadyRx());
+
+    /*
+     * Checking RX on 2
+     */
+    uavcan::CanFrame frame;
+    uavcan::MonotonicTime ts_mono;
+    uavcan::UtcTime ts_utc;
+    uavcan::CanIOFlags flags = 0;
+
+    ENFORCE(1 == if2.receive(frame, ts_mono, ts_utc, flags));
+    ENFORCE(frame == makeFrame(123, "1"));
+
+    ENFORCE(1 == if2.receive(frame, ts_mono, ts_utc, flags));
+    ENFORCE(frame == makeFrame(123 | EFF, "2"));
+
+    ENFORCE(1 == if2.receive(frame, ts_mono, ts_utc, flags));
+    ENFORCE(frame == makeFrame(456789 | EFF, "5"));
+
+    ENFORCE(1 == if2.receive(frame, ts_mono, ts_utc, flags));
+    ENFORCE(frame == makeFrame(0, "6"));
+    ENFORCE(flags == 0);
+
+    ENFORCE(!if2.hasReadyRx());
+}
+
+static void testDriver(const std::vector<std::string>& iface_names)
+{
+    /*
+     * Clocks will have some offset from the true system time
+     * SocketCAN driver must handle this correctly
+     */
+    uavcan_linux::SystemClock clock_impl(uavcan_linux::ClockAdjustmentMode::PerDriverPrivate);
+    clock_impl.adjustUtc(uavcan::UtcDuration::fromMSec(9000000));
+    const uavcan_linux::SystemClock& clock = clock_impl;
+
+    uavcan_linux::SocketCanDriver driver(clock);
+    for (auto ifn : iface_names)
+    {
+        std::cout << "Adding iface " << ifn << std::endl;
+        ENFORCE(0 == driver.addIface(ifn));
+    }
+
+    ENFORCE(-1 == driver.addIface("noif9"));
+    ENFORCE(-1 == driver.addIface("noif9"));
+    ENFORCE(-1 == driver.addIface("noif9"));
+
+    ENFORCE(driver.getNumIfaces() == iface_names.size());
+    ENFORCE(nullptr == driver.getIface(255));
+    ENFORCE(nullptr == driver.getIface(driver.getNumIfaces()));
+
+    const unsigned AllIfacesMask = (1 << driver.getNumIfaces()) - 1;
+
+    /*
+     * Send, no loopback
+     */
+    std::cout << "select() 1" << std::endl;
+    uavcan::CanSelectMasks masks;        // Driver provides masks for all available events
+    ENFORCE(driver.getNumIfaces() == driver.select(masks, tsMonoOffsetMs(1000)));
+    ENFORCE(masks.read == 0);
+    ENFORCE(masks.write == AllIfacesMask);
+
+    for (int i = 0; i < driver.getNumIfaces(); i++)
+    {
+        ENFORCE(1 == driver.getIface(i)->send(makeFrame(123, std::to_string(i)), tsMonoOffsetMs(10), 0));
+    }
+
+    std::cout << "select() 2" << std::endl;
+    ENFORCE(driver.getNumIfaces() == driver.select(masks, tsMonoOffsetMs(1000)));
+    ENFORCE(masks.read == 0);
+    ENFORCE(masks.write == AllIfacesMask);
+
+    /*
+     * Send with loopback
+     */
+    for (int i = 0; i < driver.getNumIfaces(); i++)
+    {
+        ENFORCE(1 == driver.getIface(i)->send(makeFrame(456, std::to_string(i)), tsMonoOffsetMs(10),
+                                              uavcan::CanIOFlagLoopback));
+        ENFORCE(1 == driver.getIface(i)->send(makeFrame(789, std::to_string(i)), tsMonoOffsetMs(-1), // Will timeout
+                                              uavcan::CanIOFlagLoopback));
+    }
+
+    std::cout << "select() 3" << std::endl;
+    ENFORCE(driver.getNumIfaces() == driver.select(masks, tsMonoOffsetMs(1000)));
+    ENFORCE(masks.read == AllIfacesMask);
+    ENFORCE(masks.write == AllIfacesMask);
+
+    /*
+     * Receive loopback
+     */
+    for (int i = 0; i < driver.getNumIfaces(); i++)
+    {
+        uavcan::CanFrame frame;
+        uavcan::MonotonicTime ts_mono;
+        uavcan::UtcTime ts_utc;
+        uavcan::CanIOFlags flags = 0;
+        ENFORCE(1 == driver.getIface(i)->receive(frame, ts_mono, ts_utc, flags));
+        ENFORCE(frame == makeFrame(456, std::to_string(i)));
+        ENFORCE(flags == uavcan::CanIOFlagLoopback);
+        ENFORCE((clock.getMonotonic() - ts_mono).getAbs().toMSec() < 10);
+        ENFORCE((clock.getUtc() - ts_utc).getAbs().toMSec() < 10);
+
+        ENFORCE(!driver.getIface(i)->hasPendingTx());
+        ENFORCE(!driver.getIface(i)->hasReadyRx());
+    }
+
+    std::cout << "select() 4" << std::endl;
+    masks.write = 0;
+    ENFORCE(driver.getNumIfaces() == driver.select(masks, tsMonoOffsetMs(1000)));
+    ENFORCE(masks.read == 0);
+    ENFORCE(masks.write == AllIfacesMask);
+
+    std::cout << "exit" << std::endl;
+
+    /*
+     * Error checks
+     */
+    for (int i = 0; i < driver.getNumIfaces(); i++)
+    {
+        for (auto kv : driver.getIface(i)->getErrors())
+        {
+            switch (kv.first)
+            {
+            case uavcan_linux::SocketCanError::SocketReadFailure:
+            case uavcan_linux::SocketCanError::SocketWriteFailure:
+            {
+                ENFORCE(kv.second == 0);
+                break;
+            }
+            case uavcan_linux::SocketCanError::TxTimeout:
+            {
+                ENFORCE(kv.second == 1);  // One timed out frame from the above
+                break;
+            }
+            default:
+            {
+                ENFORCE(false);
+                break;
+            }
+            }
+        }
+    }
+}
+
+int main(int argc, const char** argv)
+{
+    if (argc < 2)
+    {
+        std::cout << "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]);
+    }
+
+    testNonexistentIface();
+    testSocketRxTx(iface_names[0]);
+    testSocketFilters(iface_names[0]);
+
+    testDriver(iface_names);
+
+    return 0;
+}

libuavcan_drivers/linux/apps/test_time_sync.cpp

@@ -0,0 +1,101 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include <iostream>
+#include <cassert>
+#include <uavcan_linux/uavcan_linux.hpp>
+#include <uavcan/protocol/global_time_sync_master.hpp>
+#include <uavcan/protocol/global_time_sync_slave.hpp>
+#include "debug.hpp"
+
+
+static uavcan_linux::NodePtr initNode(const std::vector<std::string>& ifaces, uavcan::NodeID nid,
+                                      const std::string& name)
+{
+    auto node = uavcan_linux::makeNode(ifaces);
+    node->setNodeID(nid);
+    node->setName(name.c_str());
+
+    ENFORCE(0 == node->start());
+
+    uavcan::NetworkCompatibilityCheckResult init_result;
+    ENFORCE(0 == node->checkNetworkCompatibility(init_result));
+    if (!init_result.isOk())
+    {
+        throw std::runtime_error("Network conflict with node " + std::to_string(init_result.conflicting_node.get()));
+    }
+
+    node->setStatusOk();
+    return node;
+}
+
+static void runForever(const uavcan_linux::NodePtr& node)
+{
+    uavcan::GlobalTimeSyncMaster tsmaster(*node);
+    ENFORCE(0 == tsmaster.init());
+
+    uavcan::GlobalTimeSyncSlave tsslave(*node);
+    ENFORCE(0 == tsslave.start());
+
+    auto publish_sync_if_master = [&](const uavcan::TimerEvent&)
+    {
+        bool i_am_master = false;
+        if (tsslave.isActive())
+        {
+            const uavcan::NodeID master_node = tsslave.getMasterNodeID();
+            assert(master_node.isValid());
+            if (node->getNodeID() < master_node)
+            {
+                std::cout << "Overriding the lower priority master " << int(master_node.get()) << std::endl;
+                i_am_master = true;
+            }
+            else
+            {
+                std::cout << "There is other master of higher priority " << int(master_node.get()) << std::endl;
+            }
+        }
+        else
+        {
+            std::cout << "No other masters present" << std::endl;
+            i_am_master = true;
+        }
+
+        // Don't forget to disable slave adjustments if we're master
+        tsslave.suppress(i_am_master);
+
+        if (i_am_master)
+        {
+            ENFORCE(0 <= tsmaster.publish());
+        }
+    };
+
+    auto sync_publish_timer = node->makeTimer(uavcan::MonotonicDuration::fromMSec(1000), publish_sync_if_master);
+
+    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)
+{
+    if (argc < 3)
+    {
+        std::cout << "Usage:\n\t" << argv[0] << " <node-id> <can-iface-name-1> [can-iface-name-N...]" << std::endl;
+        return 1;
+    }
+    const int self_node_id = std::stoi(argv[1]);
+    std::vector<std::string> iface_names;
+    for (int i = 2; i < argc; i++)
+    {
+        iface_names.emplace_back(argv[i]);
+    }
+    uavcan_linux::NodePtr node = initNode(iface_names, self_node_id, "org.uavcan.linux_test_node_status_monitor");
+    runForever(node);
+    return 0;
+}

libuavcan_drivers/linux/apps/uavcan_nodetool.cpp

@@ -0,0 +1,281 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include <iostream>
+#include <iomanip>
+#include <thread>
+#include <mutex>
+#include <algorithm>
+#include <iterator>
+#include <uavcan_linux/uavcan_linux.hpp>
+#include <uavcan/protocol/node_status_monitor.hpp>
+#include "debug.hpp"
+
+#include <uavcan/protocol/param/GetSet.hpp>
+#include <uavcan/protocol/param/SaveErase.hpp>
+#include <uavcan/protocol/EnumerationRequest.hpp>
+#include <uavcan/equipment/hardpoint/Command.hpp>
+
+namespace
+{
+
+class StdinLineReader
+{
+    mutable std::mutex mutex_;
+    std::thread thread_;
+    std::queue<std::string> queue_;
+
+    void worker()
+    {
+        while (true)
+        {
+            std::string input;
+            std::getline(std::cin, input);
+            std::lock_guard<std::mutex> lock(mutex_);
+            queue_.push(input);
+        }
+    }
+
+public:
+    StdinLineReader()
+        : thread_(&StdinLineReader::worker, this)
+    {
+        thread_.detach();
+    }
+
+    bool hasPendingInput() const
+    {
+        std::lock_guard<std::mutex> lock(mutex_);
+        return !queue_.empty();
+    }
+
+    std::string getLine()
+    {
+        std::lock_guard<std::mutex> lock(mutex_);
+        if (queue_.empty())
+        {
+            throw std::runtime_error("Input queue is empty");
+        }
+        auto ret = queue_.front();
+        queue_.pop();
+        return ret;
+    }
+
+    std::vector<std::string> getSplitLine()
+    {
+        std::istringstream iss(getLine());
+        std::vector<std::string> out;
+        std::copy(std::istream_iterator<std::string>(iss), std::istream_iterator<std::string>(),
+                  std::back_inserter(out));
+        return out;
+    }
+};
+
+
+std::string paramValueToString(const uavcan::protocol::param::Value& value)
+{
+    if (!value.value_bool.empty())
+    {
+        return value.value_bool[0] ? "true" : "false";
+    }
+    else if (!value.value_int.empty())
+    {
+        return std::to_string(value.value_int[0]);
+    }
+    else if (!value.value_float.empty())
+    {
+        return std::to_string(value.value_float[0]);
+    }
+    else
+    {
+        return "?";
+    }
+}
+
+void printGetSetResponseHeader()
+{
+    std::cout
+        << "Name                                     Value          Default        Min            Max\n"
+        << "--------------------------------------------------------------------------------------------------"
+        << std::endl;
+}
+
+void printGetSetResponse(const uavcan::protocol::param::GetSet::Response& resp)
+{
+    std::cout << std::setw(41) << std::left << resp.name.c_str();
+    std::cout << std::setw(15) << paramValueToString(resp.value);
+    std::cout << std::setw(15) << paramValueToString(resp.default_value);
+    std::cout << std::setw(15) << paramValueToString(resp.min_value);
+    std::cout << std::setw(15) << paramValueToString(resp.max_value);
+    std::cout << std::endl;
+}
+
+uavcan_linux::NodePtr initNode(const std::vector<std::string>& ifaces, uavcan::NodeID nid, const std::string& name)
+{
+    auto node = uavcan_linux::makeNode(ifaces);
+    node->setNodeID(nid);
+    node->setName(name.c_str());
+    ENFORCE(0 == node->start());  // This node doesn't check its network compatibility
+    node->setStatusOk();
+    return node;
+}
+
+template <typename DataType>
+typename DataType::Response call(uavcan_linux::BlockingServiceClient<DataType>& client,
+                                 uavcan::NodeID server_node_id, const typename DataType::Request& request)
+{
+    const int res = client.blockingCall(server_node_id, request, uavcan::MonotonicDuration::fromMSec(100));
+    ENFORCE(res >= 0);
+    ENFORCE(client.wasSuccessful());
+    return client.getResponse();
+}
+
+void executeCommand(const uavcan_linux::NodePtr& node, const std::string& cmd,
+                    const uavcan::NodeID node_id, const std::vector<std::string>& args)
+{
+    if (cmd == "param")
+    {
+        auto client = node->makeBlockingServiceClient<uavcan::protocol::param::GetSet>();
+        printGetSetResponseHeader();
+        uavcan::protocol::param::GetSet::Request request;
+        if (args.empty())
+        {
+            while (true)
+            {
+                auto response = call(*client, node_id, request);
+                if (response.name.empty())
+                {
+                    break;
+                }
+                printGetSetResponse(response);
+                request.index++;
+            }
+        }
+        else
+        {
+            request.name = args.at(0).c_str();
+            request.value.value_float.push_back(std::stof(args.at(1)));
+            printGetSetResponse(call(*client, node_id, request));
+        }
+    }
+    else if (cmd == "param_save" || cmd == "param_erase")
+    {
+        auto client = node->makeBlockingServiceClient<uavcan::protocol::param::SaveErase>();
+        uavcan::protocol::param::SaveErase::Request request;
+        request.opcode = (cmd == "param_save") ? request.OPCODE_SAVE : request.OPCODE_ERASE;
+        std::cout << call(*client, node_id, request) << std::endl;
+    }
+    else if (cmd == "restart")
+    {
+        auto client = node->makeBlockingServiceClient<uavcan::protocol::RestartNode>();
+        uavcan::protocol::RestartNode::Request request;
+        request.magic_number = request.MAGIC_NUMBER;
+        (void)client->blockingCall(node_id, request);
+        if (client->wasSuccessful())
+        {
+            std::cout << client->getResponse() << std::endl;
+        }
+        else
+        {
+            std::cout << "<NO RESPONSE>" << std::endl;
+        }
+    }
+    else if (cmd == "info")
+    {
+        auto client = node->makeBlockingServiceClient<uavcan::protocol::GetNodeInfo>();
+        std::cout << call(*client, node_id, uavcan::protocol::GetNodeInfo::Request()) << std::endl;
+    }
+    else if (cmd == "tstat")
+    {
+        auto client = node->makeBlockingServiceClient<uavcan::protocol::GetTransportStats>();
+        std::cout << call(*client, node_id, uavcan::protocol::GetTransportStats::Request()) << std::endl;
+    }
+    else if (cmd == "hardpoint")
+    {
+        uavcan::equipment::hardpoint::Command msg;
+        msg.command = std::stoi(args.at(0));
+        auto pub = node->makePublisher<uavcan::equipment::hardpoint::Command>();
+        if (node_id.isBroadcast())
+        {
+            (void)pub->broadcast(msg);
+        }
+        else
+        {
+            (void)pub->unicast(msg, node_id);
+        }
+    }
+    else if (cmd == "enum")
+    {
+        uavcan::protocol::EnumerationRequest msg;
+        msg.node_id = std::stoi(args.at(0));
+        msg.timeout_sec = (args.size() > 1) ? std::stoi(args.at(1)) : 60;
+        std::cout << msg << std::endl;
+        auto pub = node->makePublisher<uavcan::protocol::EnumerationRequest>();
+        if (node_id.isBroadcast())
+        {
+            (void)pub->broadcast(msg);
+        }
+        else
+        {
+            (void)pub->unicast(msg, node_id);
+        }
+    }
+    else
+    {
+        std::cout << "Invalid command" << std::endl;
+    }
+}
+
+void runForever(const uavcan_linux::NodePtr& node)
+{
+    StdinLineReader stdin_reader;
+    std::cout << "> " << std::flush;
+    while (true)
+    {
+        ENFORCE(node->spin(uavcan::MonotonicDuration::fromMSec(10)) >= 0);
+        if (!stdin_reader.hasPendingInput())
+        {
+            continue;
+        }
+        const auto words = stdin_reader.getSplitLine();
+        if (words.size() >= 2)
+        {
+            const auto cmd = words.at(0);
+            const uavcan::NodeID node_id(std::stoi(words.at(1)));
+            try
+            {
+                executeCommand(node, cmd, node_id, std::vector<std::string>(words.begin() + 2, words.end()));
+            }
+            catch (std::exception& ex)
+            {
+                std::cout << "FAILURE\n" << ex.what() << std::endl;
+            }
+        }
+        else
+        {
+            std::cout << "<command> <remote node id> [args...]" << std::endl;
+        }
+        std::cout << "> " << std::flush;
+    }
+}
+
+}
+
+int main(int argc, const char** argv)
+{
+    if (argc < 3)
+    {
+        std::cout << "Usage:\n\t" << argv[0] << " <node-id> <can-iface-name-1> [can-iface-name-N...]" << std::endl;
+        return 1;
+    }
+    const int self_node_id = std::stoi(argv[1]);
+    std::vector<std::string> iface_names;
+    for (int i = 2; i < argc; i++)
+    {
+        iface_names.emplace_back(argv[i]);
+    }
+    uavcan_linux::NodePtr node = initNode(iface_names, self_node_id, "org.uavcan.linux_test_node_status_monitor");
+    runForever(node);
+    return 0;
+}

libuavcan_drivers/linux/apps/uavcan_status_monitor.cpp

@@ -0,0 +1,146 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include <iostream>
+#include <iomanip>
+#include <unordered_map>
+#include <uavcan_linux/uavcan_linux.hpp>
+#include <uavcan/protocol/node_status_monitor.hpp>
+#include "debug.hpp"
+
+struct OstreamColorizer
+{
+    enum Color
+    {
+        Red     = 31,
+        Green   = 32,
+        Yellow  = 33,
+        Blue    = 34,
+        Magenta = 35,
+        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<int, std::uint64_t> uptimes_;
+
+    virtual void handleNodeStatusMessage(const uavcan::ReceivedDataStructure<uavcan::protocol::NodeStatus>& msg)
+    {
+        uptimes_[msg.getSrcNodeID().get()] = msg.uptime_sec;
+    }
+
+    static std::pair<OstreamColorizer, std::string>
+    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::Blue), "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 color_and_string = statusCodeToColoredString(status.status_code);
+        const int nid_int = nid.get();
+        std::cout << color_and_string.first;
+        std::cout << " " << std::setw(3) << std::left << nid_int << " | "
+                  << std::setw(13) << std::left << color_and_string.second << " | "
+                  << uptimes_[nid_int];
+        std::cout << OstreamColorizer() << "\n";
+    }
+
+    void redraw(const uavcan::TimerEvent&)
+    {
+        std::cout << "\x1b\x5b\x48" << "\x1b\x5b\x32\x4a"
+                  << " NID | Status        | Uptime\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 initNode(const std::vector<std::string>& ifaces, uavcan::NodeID nid,
+                                      const std::string& name)
+{
+    auto node = uavcan_linux::makeNode(ifaces);
+    node->setNodeID(nid);
+    node->setName(name.c_str());
+    ENFORCE(0 == node->start());  // This node doesn't check its network compatibility
+    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)
+{
+    if (argc < 3)
+    {
+        std::cout << "Usage:\n\t" << argv[0] << " <node-id> <can-iface-name-1> [can-iface-name-N...]" << std::endl;
+        return 1;
+    }
+    const int self_node_id = std::stoi(argv[1]);
+    std::vector<std::string> iface_names;
+    for (int i = 2; i < argc; i++)
+    {
+        iface_names.emplace_back(argv[i]);
+    }
+    uavcan_linux::NodePtr node = initNode(iface_names, self_node_id, "org.uavcan.linux_test_node_status_monitor");
+    runForever(node);
+    return 0;
+}