test_dynamic_node_id_client.cpp

00001 /*
00002  * Copyright (C) 2015 Pavel Kirienko <pavel.kirienko@gmail.com>
00003  */
00004 
00005 #include <iostream>
00006 #include "debug.hpp"
00007 #include <uavcan/protocol/dynamic_node_id_client.hpp>
00008 #include <uavcan_linux/uavcan_linux.hpp>
00009 
00010 namespace
00011 {
00012 
00013 uavcan_linux::NodePtr initNodeWithDynamicID(const std::vector<std::string>& ifaces,
00014                                             const std::uint8_t instance_id,
00015                                             const uavcan::NodeID preferred_node_id,
00016                                             const std::string& name)
00017 {
00018     /*
00019      * Initializing the node object
00020      */
00021     auto node = uavcan_linux::makeNode(ifaces);
00022 
00023     node->setName(name.c_str());
00024     node->getLogger().setLevel(uavcan::protocol::debug::LogLevel::DEBUG);
00025 
00026     {
00027         const auto app_id = uavcan_linux::makeApplicationID(uavcan_linux::MachineIDReader().read(), name, instance_id);
00028 
00029         uavcan::protocol::HardwareVersion hwver;
00030         std::copy(app_id.begin(), app_id.end(), hwver.unique_id.begin());
00031         std::cout << hwver << std::endl;
00032 
00033         node->setHardwareVersion(hwver);
00034     }
00035 
00036     /*
00037      * Starting the node
00038      */
00039     const int start_res = node->start();
00040     ENFORCE(0 == start_res);
00041 
00042     /*
00043      * Running the dynamic node ID client until it's done
00044      */
00045     uavcan::DynamicNodeIDClient client(*node);
00046 
00047     ENFORCE(0 <= client.start (node->getNodeStatusProvider().getHardwareVersion().unique_id, preferred_node_id));
00048 
00049     std::cout << "Waiting for dynamic node ID allocation..." << std::endl;
00050 
00051     while (!client.isAllocationComplete())
00052     {
00053         const int res = node->spin(uavcan::MonotonicDuration::fromMSec(100));
00054         if (res < 0)
00055         {
00056             std::cerr << "Spin error: " << res << std::endl;
00057         }
00058     }
00059 
00060     std::cout << "Node ID " << int(client.getAllocatedNodeID().get())
00061               << " allocated by " << int(client.getAllocatorNodeID().get()) << std::endl;
00062 
00063     /*
00064      * Finishing the node initialization
00065      */
00066     node->setNodeID(client.getAllocatedNodeID());
00067 
00068     node->setModeOperational();
00069 
00070     return node;
00071 }
00072 
00073 void runForever(const uavcan_linux::NodePtr& node)
00074 {
00075     while (true)
00076     {
00077         const int res = node->spin(uavcan::MonotonicDuration::fromMSec(100));
00078         if (res < 0)
00079         {
00080             std::cerr << "Spin error: " << res << std::endl;
00081         }
00082     }
00083 }
00084 
00085 }
00086 
00087 int main(int argc, const char** argv)
00088 {
00089     try
00090     {
00091         if (argc < 3)
00092         {
00093             std::cerr << "Usage:\n\t"
00094                       << argv[0] << " <instance-id> <can-iface-name-1> [can-iface-name-N...]\n"
00095                       << "Where <instance-id> is used to augment the unique node ID and also indicates\n"
00096                       << "the preferred node ID value. Valid range is [0, 127]."
00097                       << std::endl;
00098             return 1;
00099         }
00100 
00101         const int instance_id = std::stoi(argv[1]);
00102         if (instance_id < 0 || instance_id > 127)
00103         {
00104             std::cerr << "Invalid instance ID: " << instance_id << std::endl;
00105             std::exit(1);
00106         }
00107 
00108         uavcan_linux::NodePtr node = initNodeWithDynamicID(std::vector<std::string>(argv + 2, argv + argc),
00109                                                            std::uint8_t(instance_id),
00110                                                            std::uint8_t(instance_id),
00111                                                            "org.uavcan.linux_test_dynamic_node_id_client");
00112         runForever(node);
00113 
00114         return 0;
00115     }
00116     catch (const std::exception& ex)
00117     {
00118         std::cerr << "Error: " << ex.what() << std::endl;
00119         return 1;
00120     }
00121 }