Linux test app - Dynamic node ID client

libuavcan_drivers/linux/CMakeLists.txt

@@ -60,6 +60,9 @@ target_link_libraries(test_system_utils ${UAVCAN_LIB} rt ${CMAKE_THREAD_LIBS_INI
 add_executable(test_posix apps/test_posix.cpp)
 target_link_libraries(test_posix ${UAVCAN_LIB} rt ${CMAKE_THREAD_LIBS_INIT})
 
+add_executable(test_dynamic_node_id_client apps/test_dynamic_node_id_client.cpp)
+target_link_libraries(test_dynamic_node_id_client ${UAVCAN_LIB} rt ${CMAKE_THREAD_LIBS_INIT})
+
 #
 # Tools
 #

libuavcan_drivers/linux/apps/test_dynamic_node_id_client.cpp

@@ -0,0 +1,121 @@
+/*
+ * Copyright (C) 2015 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include <iostream>
+#include "debug.hpp"
+#include <uavcan/protocol/dynamic_node_id_client.hpp>
+#include <uavcan_linux/uavcan_linux.hpp>
+
+namespace
+{
+
+uavcan_linux::NodePtr initNodeWithDynamicID(const std::vector<std::string>& ifaces,
+                                            const std::uint8_t instance_id,
+                                            const uavcan::NodeID preferred_node_id,
+                                            const std::string& name)
+{
+    /*
+     * Initializing the node object
+     */
+    auto node = uavcan_linux::makeNode(ifaces);
+
+    node->setName(name.c_str());
+    node->getLogger().setLevel(uavcan::protocol::debug::LogLevel::DEBUG);
+
+    {
+        const auto app_id = uavcan_linux::makeApplicationID(uavcan_linux::MachineIDReader().read(), name, instance_id);
+
+        uavcan::protocol::HardwareVersion hwver;
+        std::copy(app_id.begin(), app_id.end(), hwver.unique_id.begin());
+        std::cout << hwver << std::endl;
+
+        node->setHardwareVersion(hwver);
+    }
+
+    /*
+     * Starting the node
+     */
+    const int start_res = node->start();
+    ENFORCE(0 == start_res);
+
+    /*
+     * Running the dynamic node ID client until it's done
+     */
+    uavcan::DynamicNodeIDClient client(*node);
+
+    ENFORCE(0 <= client.start(node->getNodeStatusProvider().getHardwareVersion(), preferred_node_id));
+
+    std::cout << "Waiting for dynamic node ID allocation..." << std::endl;
+
+    while (!client.isAllocationComplete())
+    {
+        const int res = node->spin(uavcan::MonotonicDuration::fromMSec(100));
+        if (res < 0)
+        {
+            std::cerr << "Spin error: " << res << std::endl;
+        }
+    }
+
+    std::cout << "Node ID " << int(client.getAllocatedNodeID().get())
+              << " allocated by " << int(client.getAllocatorNodeID().get()) << std::endl;
+
+    /*
+     * Finishing the node initialization
+     */
+    node->setNodeID(client.getAllocatedNodeID());
+
+    node->setStatusOk();
+
+    return node;
+}
+
+void runForever(const uavcan_linux::NodePtr& node)
+{
+    while (true)
+    {
+        const int res = node->spin(uavcan::MonotonicDuration::fromMSec(100));
+        if (res < 0)
+        {
+            std::cerr << "Spin error: " << res << std::endl;
+        }
+    }
+}
+
+}
+
+int main(int argc, const char** argv)
+{
+    try
+    {
+        if (argc < 3)
+        {
+            std::cerr << "Usage:\n\t"
+                      << argv[0] << " <instance-id> <can-iface-name-1> [can-iface-name-N...]\n"
+                      << "Where <instance-id> is used to augment the unique node ID and also indicates\n"
+                      << "the preferred node ID value. Valid range is [0, 127]."
+                      << std::endl;
+            return 1;
+        }
+
+        const int instance_id = std::stoi(argv[1]);
+        if (instance_id < 0 || instance_id > 127)
+        {
+            std::cerr << "Invalid instance ID: " << instance_id << std::endl;
+            std::exit(1);
+        }
+
+        uavcan_linux::NodePtr node = initNodeWithDynamicID(std::vector<std::string>(argv + 2, argv + argc),
+                                                           std::uint8_t(instance_id),
+                                                           std::uint8_t(instance_id),
+                                                           "org.uavcan.linux_test_dynamic_node_id_client");
+        runForever(node);
+
+        return 0;
+    }
+    catch (const std::exception& ex)
+    {
+        std::cerr << "Error: " << ex.what() << std::endl;
+        return 1;
+    }
+}