test_node.cpp

00001 /*
00002  * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
00003  */
00004 
00005 #include <iostream>
00006 #include <uavcan_linux/uavcan_linux.hpp>
00007 #include <uavcan/protocol/node_status_monitor.hpp>
00008 #include "debug.hpp"
00009 
00010 static uavcan_linux::NodePtr initNode(const std::vector<std::string>& ifaces, uavcan::NodeID nid,
00011                                       const std::string& name)
00012 {
00013     auto node = uavcan_linux::makeNode(ifaces);
00014 
00015     /*
00016      * Configuring the node.
00017      */
00018     node->setNodeID(nid);
00019     node->setName(name.c_str());
00020 
00021     node->getLogger().setLevel(uavcan::protocol::debug::LogLevel::DEBUG);
00022 
00023     /*
00024      * Starting the node.
00025      */
00026     std::cout << "Starting the node..." << std::endl;
00027     const int start_res = node->start();
00028     std::cout << "Start returned: " << start_res << std::endl;
00029     ENFORCE(0 == start_res);
00030 
00031     std::cout << "Node started successfully" << std::endl;
00032 
00033     /*
00034      * Say Hi to the world.
00035      */
00036     node->setModeOperational();
00037     node->logInfo("init", "Hello world! I'm [%*], NID %*",
00038                   node->getNodeStatusProvider().getName().c_str(), int(node->getNodeID().get()));
00039     return node;
00040 }
00041 
00042 static void runForever(const uavcan_linux::NodePtr& node)
00043 {
00044     /*
00045      * Subscribing to the UAVCAN logging topic
00046      */
00047     auto log_handler = [](const uavcan::ReceivedDataStructure<uavcan::protocol::debug::LogMessage>& msg)
00048     {
00049         std::cout << msg << std::endl;
00050     };
00051     auto log_sub = node->makeSubscriber<uavcan::protocol::debug::LogMessage>(log_handler);
00052 
00053     /*
00054      * Printing when other nodes enter the network or change status
00055      */
00056     struct NodeStatusMonitor : public uavcan::NodeStatusMonitor
00057     {
00058         explicit NodeStatusMonitor(uavcan::INode& node) : uavcan::NodeStatusMonitor(node) { }
00059 
00060         void handleNodeStatusChange(const NodeStatusChangeEvent& event) override
00061         {
00062             std::cout << "Remote node NID " << int(event.node_id.get()) << " changed status: "
00063                       << event.old_status.toString() << " --> "
00064                       << event.status.toString() << std::endl;
00065         }
00066     };
00067 
00068     NodeStatusMonitor nsm(*node);
00069     ENFORCE(0 == nsm.start());
00070 
00071     /*
00072      * Adding a stupid timer that does nothing once a minute
00073      */
00074     auto do_nothing_once_a_minute = [&node](const uavcan::TimerEvent&)
00075     {
00076         node->logInfo("timer", "Another minute passed...");
00077         // coverity[dont_call]
00078         node->setVendorSpecificStatusCode(static_cast<std::uint16_t>(std::rand())); // Setting to an arbitrary value
00079     };
00080     auto timer = node->makeTimer(uavcan::MonotonicDuration::fromMSec(60000), do_nothing_once_a_minute);
00081 
00082     /*
00083      * Spinning forever
00084      */
00085     while (true)
00086     {
00087         const int res = node->spin(uavcan::MonotonicDuration::getInfinite());
00088         if (res < 0)
00089         {
00090             node->logError("spin", "Error %*", res);
00091         }
00092     }
00093 }
00094 
00095 int main(int argc, const char** argv)
00096 {
00097     try
00098     {
00099         if (argc < 3)
00100         {
00101             std::cerr << "Usage:\n\t" << argv[0] << " <node-id> <can-iface-name-1> [can-iface-name-N...]" << std::endl;
00102             return 1;
00103         }
00104         const int self_node_id = std::stoi(argv[1]);
00105         std::vector<std::string> iface_names;
00106         for (int i = 2; i < argc; i++)
00107         {
00108             iface_names.emplace_back(argv[i]);
00109         }
00110         uavcan_linux::NodePtr node = initNode(iface_names, self_node_id, "org.uavcan.linux_test_node");
00111         std::cout << "Node initialized successfully" << std::endl;
00112         runForever(node);
00113         return 0;
00114     }
00115     catch (const std::exception& ex)
00116     {
00117         std::cerr << "Exception: " << ex.what() << std::endl;
00118         return 1;
00119     }
00120 }