From 4f87487d46e74448588d248fa0c442a6a72ac2c4 Mon Sep 17 00:00:00 2001
From: Pavel Kirienko <pavel.kirienko@gmail.com>
Date: Tue, 1 Apr 2014 12:08:09 +0400
Subject: [PATCH] Linux test node

---
 libuavcan_drivers/linux/CMakeLists.txt     |   3 +
 libuavcan_drivers/linux/test/test_node.cpp | 132 +++++++++++++++++++++
 2 files changed, 135 insertions(+)
 create mode 100644 libuavcan_drivers/linux/test/test_node.cpp

diff --git a/libuavcan_drivers/linux/CMakeLists.txt b/libuavcan_drivers/linux/CMakeLists.txt
index dd97f9a0c7..b6d3d91013 100644
--- a/libuavcan_drivers/linux/CMakeLists.txt
+++ b/libuavcan_drivers/linux/CMakeLists.txt
@@ -36,3 +36,6 @@ target_link_libraries(test_clock ${UAVCAN_LIB} rt)
 
 add_executable(test_socket test/test_socket.cpp)
 target_link_libraries(test_socket ${UAVCAN_LIB} rt)
+
+add_executable(test_node test/test_node.cpp)
+target_link_libraries(test_node ${UAVCAN_LIB} rt)
diff --git a/libuavcan_drivers/linux/test/test_node.cpp b/libuavcan_drivers/linux/test/test_node.cpp
new file mode 100644
index 0000000000..6872264ce5
--- /dev/null
+++ b/libuavcan_drivers/linux/test/test_node.cpp
@@ -0,0 +1,132 @@
+/*
+ * 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"
+
+class LogSink : public uavcan::ILogSink
+{
+    virtual void log(const uavcan::protocol::debug::LogMessage& message)
+    {
+        std::cout << "--- LOCAL LOG ---\n"
+                  << message << "\n"
+                  << "-----------------" << std::endl;
+    }
+};
+
+static LogSink log_sink_;
+
+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);
+    node->getLogger().setExternalSink(&log_sink_);
+
+    /*
+     * Starting the node. This may take a few seconds.
+     */
+    std::cout << "Starting the node..." << std::endl;
+    uavcan::NodeInitializationResult init_result;
+    const int start_res = node->start(init_result);
+    std::cout << "Start returned: " << start_res << std::endl;
+    ENFORCE(0 == start_res);
+    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();
+    (void)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
+    {
+        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&)
+    {
+        (void)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)
+        {
+            (void)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;
+}