node_info_retriever.cpp

00001 /*
00002  * Copyright (C) 2015 Pavel Kirienko <pavel.kirienko@gmail.com>
00003  */
00004 
00005 #if __GNUC__
00006 // We need auto_ptr for compatibility reasons
00007 # pragma GCC diagnostic ignored "-Wdeprecated-declarations"
00008 # pragma GCC diagnostic ignored "-Wzero-as-null-pointer-constant"
00009 #endif
00010 
00011 #include <memory>
00012 #include <gtest/gtest.h>
00013 #include <uavcan/protocol/node_info_retriever.hpp>
00014 #include <uavcan/protocol/node_status_provider.hpp>
00015 #include "helpers.hpp"
00016 
00017 static void publishNodeStatus(PairableCanDriver& can, uavcan::NodeID node_id,
00018                               uavcan::uint32_t uptime_sec, uavcan::TransferID tid)
00019 {
00020     uavcan::protocol::NodeStatus msg;
00021     msg.health     = uavcan::protocol::NodeStatus::HEALTH_OK;
00022     msg.mode       = uavcan::protocol::NodeStatus::MODE_OPERATIONAL;
00023     msg.uptime_sec = uptime_sec;
00024     emulateSingleFrameBroadcastTransfer(can, node_id, msg, tid);
00025 }
00026 
00027 
00028 struct NodeInfoListener : public uavcan::INodeInfoListener
00029 {
00030     std::auto_ptr<uavcan::protocol::GetNodeInfo::Response> last_node_info;
00031     uavcan::NodeID last_node_id;
00032     unsigned status_message_cnt;
00033     unsigned status_change_cnt;
00034     unsigned info_unavailable_cnt;
00035 
00036     NodeInfoListener()
00037         : status_message_cnt(0)
00038         , status_change_cnt(0)
00039         , info_unavailable_cnt(0)
00040     { }
00041 
00042     virtual void handleNodeInfoRetrieved(uavcan::NodeID node_id,
00043                                          const uavcan::protocol::GetNodeInfo::Response& node_info)
00044     {
00045         last_node_id = node_id;
00046         std::cout << node_info << std::endl;
00047         last_node_info.reset(new uavcan::protocol::GetNodeInfo::Response(node_info));
00048     }
00049 
00050     virtual void handleNodeInfoUnavailable(uavcan::NodeID node_id)
00051     {
00052         std::cout << "NODE INFO FOR " << int(node_id.get()) << " IS UNAVAILABLE" << std::endl;
00053         last_node_id = node_id;
00054         info_unavailable_cnt++;
00055     }
00056 
00057     virtual void handleNodeStatusChange(const uavcan::NodeStatusMonitor::NodeStatusChangeEvent& event)
00058     {
00059         std::cout << "NODE " << int(event.node_id.get()) << " STATUS CHANGE: "
00060                   << event.old_status.toString() << " --> " << event.status.toString() << std::endl;
00061         status_change_cnt++;
00062     }
00063 
00064     virtual void handleNodeStatusMessage(const uavcan::ReceivedDataStructure<uavcan::protocol::NodeStatus>& msg)
00065     {
00066         std::cout << msg << std::endl;
00067         status_message_cnt++;
00068     }
00069 };
00070 
00071 
00072 TEST(NodeInfoRetriever, Basic)
00073 {
00074     uavcan::GlobalDataTypeRegistry::instance().reset();
00075     uavcan::DefaultDataTypeRegistrator<uavcan::protocol::NodeStatus> _reg1;
00076     uavcan::DefaultDataTypeRegistrator<uavcan::protocol::GetNodeInfo> _reg2;
00077 
00078     InterlinkedTestNodesWithSysClock nodes;
00079 
00080     uavcan::NodeInfoRetriever retr(nodes.a);
00081     std::cout << "sizeof(uavcan::NodeInfoRetriever): " << sizeof(uavcan::NodeInfoRetriever) << std::endl;
00082     std::cout << "sizeof(uavcan::ServiceClient<uavcan::protocol::GetNodeInfo>): "
00083         << sizeof(uavcan::ServiceClient<uavcan::protocol::GetNodeInfo>) << std::endl;
00084 
00085     std::auto_ptr<uavcan::NodeStatusProvider> provider(new uavcan::NodeStatusProvider(nodes.b));
00086 
00087     NodeInfoListener listener;
00088 
00089     /*
00090      * Initialization
00091      */
00092     ASSERT_LE(0, retr.start());
00093 
00094     retr.removeListener(&listener);     // Does nothing
00095     retr.addListener(&listener);
00096     retr.addListener(&listener);
00097     retr.addListener(&listener);
00098     ASSERT_EQ(1, retr.getNumListeners());
00099 
00100     uavcan::protocol::HardwareVersion hwver;
00101     hwver.unique_id[0] = 123;
00102     hwver.unique_id[4] = 213;
00103     hwver.unique_id[8] = 45;
00104 
00105     provider->setName("Ivan");
00106     provider->setHardwareVersion(hwver);
00107 
00108     ASSERT_LE(0, provider->startAndPublish());
00109 
00110     ASSERT_FALSE(retr.isRetrievingInProgress());
00111     ASSERT_EQ(0, retr.getNumPendingRequests());
00112 
00113     EXPECT_EQ(40, retr.getRequestInterval().toMSec());  // Default
00114 
00115     /*
00116      * Waiting for discovery
00117      */
00118     nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(50));
00119     ASSERT_TRUE(retr.isRetrievingInProgress());
00120     nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1500));
00121     ASSERT_FALSE(retr.isRetrievingInProgress());
00122 
00123     ASSERT_EQ(2, listener.status_message_cnt);
00124     ASSERT_EQ(1, listener.status_change_cnt);
00125     ASSERT_EQ(0, listener.info_unavailable_cnt);
00126     ASSERT_TRUE(listener.last_node_info.get());
00127     ASSERT_EQ(uavcan::NodeID(2), listener.last_node_id);
00128     ASSERT_EQ("Ivan", listener.last_node_info->name);
00129     ASSERT_TRUE(hwver == listener.last_node_info->hardware_version);
00130 
00131     provider.reset();   // Moving the provider out of the way; its entry will timeout meanwhile
00132 
00133     /*
00134      * Declaring a bunch of different nodes that don't support GetNodeInfo
00135      */
00136     ASSERT_FALSE(retr.isRetrievingInProgress());
00137 
00138     retr.setNumRequestAttempts(3);
00139 
00140     uavcan::TransferID tid;
00141 
00142     publishNodeStatus(nodes.can_a, uavcan::NodeID(10), 10, tid);
00143     publishNodeStatus(nodes.can_a, uavcan::NodeID(11), 10, tid);
00144     publishNodeStatus(nodes.can_a, uavcan::NodeID(12), 10, tid);
00145 
00146     nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(40));
00147     ASSERT_LE(1, retr.getNumPendingRequests());
00148     nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(40));
00149     ASSERT_LE(2, retr.getNumPendingRequests());
00150     nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(40));
00151     ASSERT_EQ(3, retr.getNumPendingRequests());
00152     nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1000));
00153     ASSERT_TRUE(retr.isRetrievingInProgress());
00154 
00155     tid.increment();
00156     publishNodeStatus(nodes.can_a, uavcan::NodeID(10), 11, tid);
00157     publishNodeStatus(nodes.can_a, uavcan::NodeID(11), 11, tid);
00158     publishNodeStatus(nodes.can_a, uavcan::NodeID(12), 11, tid);
00159 
00160     nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(40));
00161     ASSERT_LE(1, retr.getNumPendingRequests());
00162     nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(40));
00163     ASSERT_LE(2, retr.getNumPendingRequests());
00164     nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(40));
00165     ASSERT_EQ(3, retr.getNumPendingRequests());
00166     nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1000));
00167     ASSERT_TRUE(retr.isRetrievingInProgress());
00168 
00169     tid.increment();
00170     publishNodeStatus(nodes.can_a, uavcan::NodeID(10), 12, tid);
00171     publishNodeStatus(nodes.can_a, uavcan::NodeID(11), 12, tid);
00172     publishNodeStatus(nodes.can_a, uavcan::NodeID(12), 10, tid);     // Reset
00173 
00174     nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(40));
00175     ASSERT_LE(1, retr.getNumPendingRequests());
00176     nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(40));
00177     ASSERT_LE(2, retr.getNumPendingRequests());
00178     nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(40));
00179     ASSERT_EQ(3, retr.getNumPendingRequests());
00180     nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1000));
00181     ASSERT_TRUE(retr.isRetrievingInProgress());
00182 
00183     EXPECT_EQ(11, listener.status_message_cnt);
00184     EXPECT_EQ(5, listener.status_change_cnt);           // node 2 online/offline + 3 test nodes above
00185     EXPECT_EQ(2, listener.info_unavailable_cnt);
00186 
00187     tid.increment();
00188     publishNodeStatus(nodes.can_a, uavcan::NodeID(12), 11, tid);
00189 
00190     nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(40));
00191     ASSERT_EQ(1, retr.getNumPendingRequests());
00192     nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(40));
00193     ASSERT_EQ(1, retr.getNumPendingRequests());                         // Still one because two went offline
00194     nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1200));
00195     ASSERT_TRUE(retr.isRetrievingInProgress());
00196 
00197     tid.increment();
00198     publishNodeStatus(nodes.can_a, uavcan::NodeID(12), 12, tid);
00199 
00200     nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1200));
00201     ASSERT_FALSE(retr.isRetrievingInProgress());                // Out of attempts, stopping
00202     ASSERT_EQ(0, retr.getNumPendingRequests());
00203 
00204     EXPECT_EQ(13, listener.status_message_cnt);
00205     EXPECT_EQ(7, listener.status_change_cnt);        // node 2 online/offline + 2 test nodes above online/offline + 1
00206     EXPECT_EQ(3, listener.info_unavailable_cnt);
00207 
00208     /*
00209      * Forcing the class to forget everything
00210      */
00211     std::cout << "Invalidation" << std::endl;
00212 
00213     retr.invalidateAll();
00214 
00215     ASSERT_FALSE(retr.isRetrievingInProgress());
00216     ASSERT_EQ(0, retr.getNumPendingRequests());
00217 
00218     tid.increment();
00219     publishNodeStatus(nodes.can_a, uavcan::NodeID(10), 60, tid);
00220     publishNodeStatus(nodes.can_a, uavcan::NodeID(11), 60, tid);
00221     publishNodeStatus(nodes.can_a, uavcan::NodeID(12), 60, tid);
00222 
00223     nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(200));
00224 
00225     ASSERT_TRUE(retr.isRetrievingInProgress());
00226     ASSERT_EQ(3, retr.getNumPendingRequests());
00227 }
00228 
00229 
00230 TEST(NodeInfoRetriever, MaxConcurrentRequests)
00231 {
00232     uavcan::GlobalDataTypeRegistry::instance().reset();
00233     uavcan::DefaultDataTypeRegistrator<uavcan::protocol::NodeStatus> _reg1;
00234     uavcan::DefaultDataTypeRegistrator<uavcan::protocol::GetNodeInfo> _reg2;
00235 
00236     InterlinkedTestNodesWithSysClock nodes;
00237 
00238     uavcan::NodeInfoRetriever retr(nodes.a);
00239     std::cout << "sizeof(uavcan::NodeInfoRetriever): " << sizeof(uavcan::NodeInfoRetriever) << std::endl;
00240     std::cout << "sizeof(uavcan::ServiceClient<uavcan::protocol::GetNodeInfo>): "
00241         << sizeof(uavcan::ServiceClient<uavcan::protocol::GetNodeInfo>) << std::endl;
00242 
00243     NodeInfoListener listener;
00244 
00245     /*
00246      * Initialization
00247      */
00248     ASSERT_LE(0, retr.start());
00249 
00250     retr.addListener(&listener);
00251     ASSERT_EQ(1, retr.getNumListeners());
00252 
00253     ASSERT_FALSE(retr.isRetrievingInProgress());
00254     ASSERT_EQ(0, retr.getNumPendingRequests());
00255 
00256     ASSERT_EQ(40, retr.getRequestInterval().toMSec());
00257 
00258     const unsigned MaxPendingRequests = 26;             // See class docs
00259     const unsigned MinPendingRequestsAtFullLoad = 12;
00260 
00261     /*
00262      * Sending a lot of requests, making sure that the number of concurrent calls does not exceed the specified limit.
00263      */
00264     for (uint8_t node_id = 1U; node_id <= 127U; node_id++)
00265     {
00266         publishNodeStatus(nodes.can_a, node_id, 0, uavcan::TransferID());
00267         nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(10));
00268         ASSERT_GE(MaxPendingRequests, retr.getNumPendingRequests());
00269         ASSERT_TRUE(retr.isRetrievingInProgress());
00270     }
00271 
00272     ASSERT_GE(MaxPendingRequests, retr.getNumPendingRequests());
00273     ASSERT_LE(MinPendingRequestsAtFullLoad, retr.getNumPendingRequests());
00274 
00275     for (int i = 0; i < 8; i++)      // Approximate
00276     {
00277         nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(35));
00278         std::cout << "!!! SPIN " << i << " COMPLETE" << std::endl;
00279 
00280         ASSERT_GE(MaxPendingRequests, retr.getNumPendingRequests());
00281         ASSERT_LE(MinPendingRequestsAtFullLoad, retr.getNumPendingRequests());
00282 
00283         ASSERT_TRUE(retr.isRetrievingInProgress());
00284     }
00285 
00286     ASSERT_LT(0, retr.getNumPendingRequests());
00287     ASSERT_TRUE(retr.isRetrievingInProgress());
00288 
00289     nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(5000));
00290 
00291     ASSERT_EQ(0, retr.getNumPendingRequests());
00292     ASSERT_FALSE(retr.isRetrievingInProgress());
00293 }