test_file_server.cpp

00001 /*
00002  * Copyright (C) 2015 Pavel Kirienko <pavel.kirienko@gmail.com>
00003  */
00004 
00005 #include <iostream>
00006 #include <string>
00007 #include <cstdlib>
00008 #include <cstdio>
00009 #include <sys/types.h>
00010 #include <unistd.h>
00011 #include "debug.hpp"
00012 // UAVCAN
00013 #include <uavcan/protocol/file_server.hpp>
00014 // UAVCAN Linux drivers
00015 #include <uavcan_linux/uavcan_linux.hpp>
00016 // UAVCAN POSIX drivers
00017 #include <uavcan_posix/basic_file_server_backend.hpp>
00018 #include <uavcan_posix/firmware_version_checker.hpp>  // Compilability test
00019 
00020 namespace
00021 {
00022 
00023 uavcan_linux::NodePtr initNode(const std::vector<std::string>& ifaces, uavcan::NodeID nid, const std::string& name)
00024 {
00025     auto node = uavcan_linux::makeNode(ifaces);
00026 
00027     node->setNodeID(nid);
00028     node->setName(name.c_str());
00029     node->getLogger().setLevel(uavcan::protocol::debug::LogLevel::DEBUG);
00030 
00031     {
00032         const auto app_id = uavcan_linux::makeApplicationID(uavcan_linux::MachineIDReader().read(), name, nid.get());
00033 
00034         uavcan::protocol::HardwareVersion hwver;
00035         std::copy(app_id.begin(), app_id.end(), hwver.unique_id.begin());
00036         std::cout << hwver << std::endl;
00037 
00038         node->setHardwareVersion(hwver);
00039     }
00040 
00041     const int start_res = node->start();
00042     ENFORCE(0 == start_res);
00043 
00044     node->setModeOperational();
00045 
00046     return node;
00047 }
00048 
00049 void runForever(const uavcan_linux::NodePtr& node)
00050 {
00051     uavcan_posix::BasicFileServerBackend backend(*node);
00052 
00053     uavcan::FileServer server(*node, backend);
00054 
00055     const int server_init_res = server.start();
00056     if (server_init_res < 0)
00057     {
00058         throw std::runtime_error("Failed to start the server; error " + std::to_string(server_init_res));
00059     }
00060 
00061     while (true)
00062     {
00063         const int res = node->spin(uavcan::MonotonicDuration::fromMSec(100));
00064         if (res < 0)
00065         {
00066             std::cerr << "Spin error: " << res << std::endl;
00067         }
00068     }
00069 }
00070 
00071 struct Options
00072 {
00073     uavcan::NodeID node_id;
00074     std::vector<std::string> ifaces;
00075 };
00076 
00077 Options parseOptions(int argc, const char** argv)
00078 {
00079     const char* const executable_name = *argv++;
00080     argc--;
00081 
00082     const auto enforce = [executable_name](bool condition, const char* error_text) {
00083         if (!condition)
00084         {
00085             std::cerr << error_text << "\n"
00086                       << "Usage:\n\t"
00087                       << executable_name
00088                       << " <node-id> <can-iface-name-1> [can-iface-name-N...]"
00089                       << std::endl;
00090             std::exit(1);
00091         }
00092     };
00093 
00094     enforce(argc >= 2, "Not enough arguments");
00095 
00096     /*
00097      * Node ID is always at the first position
00098      */
00099     argc--;
00100     const int node_id = std::stoi(*argv++);
00101     enforce(node_id >= 1 && node_id <= 127, "Invalid node ID");
00102 
00103     Options out;
00104     out.node_id = uavcan::NodeID(std::uint8_t(node_id));
00105 
00106     while (argc --> 0)
00107     {
00108         const std::string token(*argv++);
00109 
00110         if (token[0] != '-')
00111         {
00112             out.ifaces.push_back(token);
00113         }
00114         else
00115         {
00116             enforce(false, "Unexpected argument");
00117         }
00118     }
00119 
00120     return out;
00121 }
00122 
00123 }
00124 
00125 int main(int argc, const char** argv)
00126 {
00127     try
00128     {
00129         auto options = parseOptions(argc, argv);
00130 
00131         std::cout << "Self node ID: " << int(options.node_id.get()) << "\n"
00132                      "Num ifaces:   " << options.ifaces.size() << "\n"
00133 #ifdef NDEBUG
00134                      "Build mode:   Release"
00135 #else
00136                      "Build mode:   Debug"
00137 #endif
00138                      << std::endl;
00139 
00140         auto node = initNode(options.ifaces, options.node_id, "org.uavcan.linux_test_file_server");
00141         runForever(node);
00142         return 0;
00143     }
00144     catch (const std::exception& ex)
00145     {
00146         std::cerr << "Error: " << ex.what() << std::endl;
00147         return 1;
00148     }
00149 }