uavcan_nodetool.cpp

00001 /*
00002  * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
00003  */
00004 
00005 #include <iostream>
00006 #include <iomanip>
00007 #include <thread>
00008 #include <mutex>
00009 #include <map>
00010 #include <algorithm>
00011 #include <iterator>
00012 #include <uavcan_linux/uavcan_linux.hpp>
00013 #include <uavcan/protocol/node_status_monitor.hpp>
00014 #include "debug.hpp"
00015 
00016 #include <uavcan/protocol/param/GetSet.hpp>
00017 #include <uavcan/protocol/param/ExecuteOpcode.hpp>
00018 #include <uavcan/equipment/hardpoint/Command.hpp>
00019 
00020 namespace
00021 {
00022 
00023 class StdinLineReader
00024 {
00025     mutable std::mutex mutex_;
00026     std::thread thread_;
00027     std::queue<std::string> queue_;
00028 
00029     void worker()
00030     {
00031         while (true)
00032         {
00033             std::string input;
00034             std::getline(std::cin, input);
00035             std::lock_guard<std::mutex> lock(mutex_);
00036             queue_.push(input);
00037         }
00038     }
00039 
00040 public:
00041     StdinLineReader()
00042         : thread_(&StdinLineReader::worker, this)
00043     {
00044         thread_.detach();
00045     }
00046 
00047     bool hasPendingInput() const
00048     {
00049         std::lock_guard<std::mutex> lock(mutex_);
00050         return !queue_.empty();
00051     }
00052 
00053     std::string getLine()
00054     {
00055         std::lock_guard<std::mutex> lock(mutex_);
00056         if (queue_.empty())
00057         {
00058             throw std::runtime_error("Input queue is empty");
00059         }
00060         auto ret = queue_.front();
00061         queue_.pop();
00062         return ret;
00063     }
00064 
00065     std::vector<std::string> getSplitLine()
00066     {
00067         std::istringstream iss(getLine());
00068         std::vector<std::string> out;
00069         std::copy(std::istream_iterator<std::string>(iss), std::istream_iterator<std::string>(),
00070                   std::back_inserter(out));
00071         return out;
00072     }
00073 };
00074 
00075 uavcan_linux::NodePtr initNode(const std::vector<std::string>& ifaces, uavcan::NodeID nid, const std::string& name)
00076 {
00077     auto node = uavcan_linux::makeNode(ifaces, name.c_str(),
00078                                        uavcan::protocol::SoftwareVersion(), uavcan::protocol::HardwareVersion(),
00079                                        nid);
00080     node->setModeOperational();
00081     return node;
00082 }
00083 
00084 template <typename DataType>
00085 typename DataType::Response call(uavcan_linux::BlockingServiceClient<DataType>& client,
00086                                  uavcan::NodeID server_node_id, const typename DataType::Request& request)
00087 {
00088     const int res = client.blockingCall(server_node_id, request, uavcan::MonotonicDuration::fromMSec(100));
00089     ENFORCE(res >= 0);
00090     ENFORCE(client.wasSuccessful());
00091     return client.getResponse();
00092 }
00093 
00094 /*
00095  * Command table.
00096  * The structure is:
00097  *      command_name : (command_usage_info, command_entry_point)
00098  * This code was written while listening to some bad dubstep so I'm not sure about its quality.
00099  */
00100 const std::map<std::string,
00101                std::pair<std::string,
00102                          std::function<void(const uavcan_linux::NodePtr&, const uavcan::NodeID,
00103                                             const std::vector<std::string>&)>
00104                         >
00105               > commands =
00106 {
00107     {
00108         "param",
00109         {
00110             "No arguments supplied - requests all params from a remote node\n"
00111             "<param_name> <param_value> - assigns parameter <param_name> to value <param_value>",
00112             [](const uavcan_linux::NodePtr& node, const uavcan::NodeID node_id, const std::vector<std::string>& args)
00113             {
00114                 auto client = node->makeBlockingServiceClient<uavcan::protocol::param::GetSet>();
00115                 uavcan::protocol::param::GetSet::Request request;
00116                 if (args.empty())
00117                 {
00118                     while (true)
00119                     {
00120                         auto response = call(*client, node_id, request);
00121                         if (response.name.empty())
00122                         {
00123                             break;
00124                         }
00125                         std::cout
00126                             << response
00127                             << "\n" << std::string(80, '-')
00128                             << std::endl;
00129                         request.index++;
00130                     }
00131                 }
00132                 else
00133                 {
00134                     request.name = args.at(0).c_str();
00135                     // TODO: add support for string parameters
00136                     request.value.to<uavcan::protocol::param::Value::Tag::real_value>() = std::stof(args.at(1));
00137                     std::cout << call(*client, node_id, request) << std::endl;
00138                 }
00139             }
00140         }
00141     },
00142     {
00143         "param_save",
00144         {
00145             "Calls uavcan.protocol.param.ExecuteOpcode on a remote node with OPCODE_SAVE",
00146             [](const uavcan_linux::NodePtr& node, const uavcan::NodeID node_id, const std::vector<std::string>&)
00147             {
00148                 auto client = node->makeBlockingServiceClient<uavcan::protocol::param::ExecuteOpcode>();
00149                 uavcan::protocol::param::ExecuteOpcode::Request request;
00150                 request.opcode = request.OPCODE_SAVE;
00151                 std::cout << call(*client, node_id, request) << std::endl;
00152             }
00153         }
00154     },
00155     {
00156         "param_erase",
00157         {
00158             "Calls uavcan.protocol.param.ExecuteOpcode on a remote node with OPCODE_ERASE",
00159             [](const uavcan_linux::NodePtr& node, const uavcan::NodeID node_id, const std::vector<std::string>&)
00160             {
00161                 auto client = node->makeBlockingServiceClient<uavcan::protocol::param::ExecuteOpcode>();
00162                 uavcan::protocol::param::ExecuteOpcode::Request request;
00163                 request.opcode = request.OPCODE_ERASE;
00164                 std::cout << call(*client, node_id, request) << std::endl;
00165             }
00166         }
00167     },
00168     {
00169         "restart",
00170         {
00171             "Restarts a remote node using uavcan.protocol.RestartNode",
00172             [](const uavcan_linux::NodePtr& node, const uavcan::NodeID node_id, const std::vector<std::string>&)
00173             {
00174                 auto client = node->makeBlockingServiceClient<uavcan::protocol::RestartNode>();
00175                 uavcan::protocol::RestartNode::Request request;
00176                 request.magic_number = request.MAGIC_NUMBER;
00177                 (void)client->blockingCall(node_id, request);
00178                 if (client->wasSuccessful())
00179                 {
00180                     std::cout << client->getResponse() << std::endl;
00181                 }
00182                 else
00183                 {
00184                     std::cout << "<NO RESPONSE>" << std::endl;
00185                 }
00186             }
00187         }
00188     },
00189     {
00190         "info",
00191         {
00192             "Calls uavcan.protocol.GetNodeInfo on a remote node",
00193             [](const uavcan_linux::NodePtr& node, const uavcan::NodeID node_id, const std::vector<std::string>&)
00194             {
00195                 auto client = node->makeBlockingServiceClient<uavcan::protocol::GetNodeInfo>();
00196                 std::cout << call(*client, node_id, uavcan::protocol::GetNodeInfo::Request()) << std::endl;
00197             }
00198         }
00199     },
00200     {
00201         "transport_stats",
00202         {
00203             "Calls uavcan.protocol.GetTransportStats on a remote node",
00204             [](const uavcan_linux::NodePtr& node, const uavcan::NodeID node_id, const std::vector<std::string>&)
00205             {
00206                 auto client = node->makeBlockingServiceClient<uavcan::protocol::GetTransportStats>();
00207                 std::cout << call(*client, node_id, uavcan::protocol::GetTransportStats::Request()) << std::endl;
00208             }
00209         }
00210     },
00211     {
00212         "hardpoint",
00213         {
00214             "Publishes uavcan.equipment.hardpoint.Command\n"
00215             "Expected argument: command",
00216             [](const uavcan_linux::NodePtr& node, const uavcan::NodeID, const std::vector<std::string>& args)
00217             {
00218                 uavcan::equipment::hardpoint::Command msg;
00219                 msg.command = std::stoi(args.at(0));
00220                 auto pub = node->makePublisher<uavcan::equipment::hardpoint::Command>();
00221                 (void)pub->broadcast(msg);
00222             }
00223         }
00224     }
00225 };
00226 
00227 void runForever(const uavcan_linux::NodePtr& node)
00228 {
00229     StdinLineReader stdin_reader;
00230     std::cout << "> " << std::flush;
00231     while (true)
00232     {
00233         ENFORCE(node->spin(uavcan::MonotonicDuration::fromMSec(10)) >= 0);
00234         if (!stdin_reader.hasPendingInput())
00235         {
00236             continue;
00237         }
00238         const auto words = stdin_reader.getSplitLine();
00239         bool command_is_known = false;
00240 
00241         try
00242         {
00243             if (words.size() >= 2)
00244             {
00245                 const auto cmd = words.at(0);
00246                 const uavcan::NodeID node_id(std::stoi(words.at(1)));
00247                 auto it = commands.find(cmd);
00248                 if (it != std::end(commands))
00249                 {
00250                     command_is_known = true;
00251                     it->second.second(node, node_id, std::vector<std::string>(words.begin() + 2, words.end()));
00252                 }
00253             }
00254         }
00255         catch (std::exception& ex)
00256         {
00257             std::cout << "FAILURE\n" << ex.what() << std::endl;
00258         }
00259 
00260         if (!command_is_known)
00261         {
00262             std::cout << "<command> <remote node id> [args...]\n";
00263             std::cout << "Say 'help' to get help.\n";     // I'll show myself out.
00264 
00265             if (!words.empty() && words.at(0) == "help")
00266             {
00267                 std::cout << "Usage:\n\n";
00268                 for (auto& cmd : commands)
00269                 {
00270                     std::cout << cmd.first << "\n" << cmd.second.first << "\n\n";
00271                 }
00272             }
00273         }
00274         std::cout << "> " << std::flush;
00275     }
00276 }
00277 
00278 }
00279 
00280 int main(int argc, const char** argv)
00281 {
00282     try
00283     {
00284         if (argc < 3)
00285         {
00286             std::cerr << "Usage:\n\t" << argv[0] << " <node-id> <can-iface-name-1> [can-iface-name-N...]" << std::endl;
00287             return 1;
00288         }
00289         const int self_node_id = std::stoi(argv[1]);
00290         const std::vector<std::string> iface_names(argv + 2, argv + argc);
00291         uavcan_linux::NodePtr node = initNode(iface_names, self_node_id, "org.uavcan.linux_app.nodetool");
00292         runForever(node);
00293         return 0;
00294     }
00295     catch (const std::exception& ex)
00296     {
00297         std::cerr << "Error: " << ex.what() << std::endl;
00298         return 1;
00299     }
00300 }