node_discoverer.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 <gtest/gtest.h>
00012 #include <vector>
00013 #include <uavcan/protocol/dynamic_node_id_server/node_discoverer.hpp>
00014 #include <uavcan/node/publisher.hpp>
00015 #include "event_tracer.hpp"
00016 #include "get_node_info_mock_server.hpp"
00017 #include "../helpers.hpp"
00018 
00019 using namespace uavcan::dynamic_node_id_server;
00020 
00021 
00022 class NodeDiscoveryHandler : public uavcan::dynamic_node_id_server::INodeDiscoveryHandler
00023 {
00024 public:
00025     struct NodeInfo
00026     {
00027         UniqueID unique_id;
00028         uavcan::NodeID node_id;
00029         bool committed;
00030 
00031         NodeInfo() : committed(false) { }
00032     };
00033 
00034     bool can_discover;
00035     std::vector<NodeInfo> nodes;
00036 
00037     NodeDiscoveryHandler() : can_discover(false) { }
00038 
00039     virtual bool canDiscoverNewNodes() const
00040     {
00041         return can_discover;
00042     }
00043 
00044     virtual NodeAwareness checkNodeAwareness(uavcan::NodeID node_id) const
00045     {
00046         const NodeInfo* const ni = const_cast<NodeDiscoveryHandler*>(this)->findNode(node_id);
00047         if (ni == UAVCAN_NULLPTR)
00048         {
00049             return NodeAwarenessUnknown;
00050         }
00051         return ni->committed ? NodeAwarenessKnownAndCommitted : NodeAwarenessKnownButNotCommitted;
00052     }
00053 
00054     virtual void handleNewNodeDiscovery(const UniqueID* unique_id_or_null, uavcan::NodeID node_id)
00055     {
00056         NodeInfo info;
00057         if (unique_id_or_null != UAVCAN_NULLPTR)
00058         {
00059             info.unique_id = *unique_id_or_null;
00060         }
00061         info.node_id = node_id;
00062         nodes.push_back(info);
00063     }
00064 
00065     NodeInfo* findNode(const UniqueID& unique_id)
00066     {
00067         for (unsigned i = 0; i < nodes.size(); i++)
00068         {
00069             if (nodes.at(i).unique_id == unique_id)
00070             {
00071                 return &nodes.at(i);
00072             }
00073         }
00074         return UAVCAN_NULLPTR;
00075     }
00076 
00077     NodeInfo* findNode(const uavcan::NodeID& node_id)
00078     {
00079         for (unsigned i = 0; i < nodes.size(); i++)
00080         {
00081             if (nodes.at(i).node_id == node_id)
00082             {
00083                 return &nodes.at(i);
00084             }
00085         }
00086         return UAVCAN_NULLPTR;
00087     }
00088 };
00089 
00090 
00091 TEST(dynamic_node_id_server_NodeDiscoverer, Basic)
00092 {
00093     using namespace uavcan::protocol::dynamic_node_id::server;
00094 
00095     uavcan::GlobalDataTypeRegistry::instance().reset();
00096     uavcan::DefaultDataTypeRegistrator<uavcan::protocol::NodeStatus> _reg1;
00097     uavcan::DefaultDataTypeRegistrator<uavcan::protocol::GetNodeInfo> _reg2;
00098 
00099     EventTracer tracer;
00100     InterlinkedTestNodesWithSysClock nodes;
00101     NodeDiscoveryHandler handler;
00102 
00103     NodeDiscoverer disc(nodes.a, tracer, handler);
00104 
00105     /*
00106      * Initialization
00107      */
00108     ASSERT_LE(0, disc.init(uavcan::TransferPriority::OneHigherThanLowest));
00109 
00110     ASSERT_FALSE(disc.hasUnknownNodes());
00111 
00112     /*
00113      * Publishing NodeStatus, discovery is disabled
00114      */
00115     std::cout << "!!! Publishing NodeStatus, discovery is disabled" << std::endl;
00116     handler.can_discover = false;
00117 
00118     uavcan::Publisher<uavcan::protocol::NodeStatus> node_status_pub(nodes.b);
00119     ASSERT_LE(0, node_status_pub.init());
00120 
00121     uavcan::protocol::NodeStatus node_status;
00122     node_status.uptime_sec = 0;
00123     ASSERT_LE(0, node_status_pub.broadcast(node_status));
00124 
00125     nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1100));
00126 
00127     ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryNewNodeFound));
00128     ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryTimerStart));   // The timer runs as long as there are unknown nodes
00129     ASSERT_EQ(0, tracer.countEvents(TraceDiscoveryGetNodeInfoRequest)); // Querying is disabled!
00130     ASSERT_EQ(0, tracer.countEvents(TraceDiscoveryGetNodeInfoFailure));
00131     ASSERT_TRUE(disc.hasUnknownNodes());
00132 
00133     /*
00134      * Enabling discovery - the querying will continue despite the fact that NodeStatus messages are not arriving
00135      */
00136     std::cout << "!!! Enabling discovery" << std::endl;
00137     handler.can_discover = true;
00138 
00139     nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1150));
00140 
00141     ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryNewNodeFound));
00142     ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryTimerStart));
00143     ASSERT_EQ(0, tracer.countEvents(TraceDiscoveryTimerStop));
00144     ASSERT_EQ(2, tracer.countEvents(TraceDiscoveryGetNodeInfoRequest));
00145     ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryGetNodeInfoFailure));
00146     ASSERT_TRUE(disc.hasUnknownNodes());
00147 
00148     /*
00149      * Publishing NodeStatus
00150      */
00151     std::cout << "!!! Publishing NodeStatus" << std::endl;
00152 
00153     node_status.uptime_sec += 5U;
00154     ASSERT_LE(0, node_status_pub.broadcast(node_status));
00155 
00156     nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1250));
00157 
00158     ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryNewNodeFound));
00159     ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryTimerStart));
00160     ASSERT_EQ(0, tracer.countEvents(TraceDiscoveryTimerStop));
00161     ASSERT_EQ(3, tracer.countEvents(TraceDiscoveryGetNodeInfoRequest));
00162     ASSERT_EQ(2, tracer.countEvents(TraceDiscoveryGetNodeInfoFailure));
00163     ASSERT_TRUE(disc.hasUnknownNodes());
00164 
00165     /*
00166      * Publishing NodeStatus, discovery is enabled, GetNodeInfo mock server is initialized
00167      */
00168     std::cout << "!!! Publishing NodeStatus, discovery is enabled, GetNodeInfo mock server is initialized" << std::endl;
00169 
00170     GetNodeInfoMockServer get_node_info_server(nodes.b);
00171     get_node_info_server.response.hardware_version.unique_id[0] = 123;  // Arbitrary data
00172     get_node_info_server.response.hardware_version.unique_id[6] = 213;
00173     get_node_info_server.response.hardware_version.unique_id[14] = 52;
00174     ASSERT_LE(0, get_node_info_server.start());
00175 
00176     nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1000));
00177 
00178     ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryNewNodeFound));
00179     ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryTimerStart));
00180     ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryTimerStop));
00181     ASSERT_EQ(4, tracer.countEvents(TraceDiscoveryGetNodeInfoRequest));
00182     ASSERT_EQ(3, tracer.countEvents(TraceDiscoveryGetNodeInfoFailure));
00183     ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryNodeFinalized));
00184     ASSERT_FALSE(disc.hasUnknownNodes());
00185 
00186     /*
00187      * Checking the results
00188      */
00189     ASSERT_TRUE(handler.findNode(get_node_info_server.response.hardware_version.unique_id));
00190     ASSERT_EQ(2, handler.findNode(get_node_info_server.response.hardware_version.unique_id)->node_id.get());
00191 }
00192 
00193 
00194 TEST(dynamic_node_id_server_NodeDiscoverer, RestartAndMaxAttempts)
00195 {
00196     using namespace uavcan::protocol::dynamic_node_id::server;
00197 
00198     uavcan::GlobalDataTypeRegistry::instance().reset();
00199     uavcan::DefaultDataTypeRegistrator<uavcan::protocol::NodeStatus> _reg1;
00200     uavcan::DefaultDataTypeRegistrator<uavcan::protocol::GetNodeInfo> _reg2;
00201 
00202     EventTracer tracer;
00203     InterlinkedTestNodesWithSysClock nodes;
00204     NodeDiscoveryHandler handler;
00205 
00206     NodeDiscoverer disc(nodes.a, tracer, handler);
00207 
00208     /*
00209      * Initialization
00210      */
00211     ASSERT_LE(0, disc.init(uavcan::TransferPriority::OneHigherThanLowest));
00212 
00213     ASSERT_FALSE(disc.hasUnknownNodes());
00214 
00215     /*
00216      * Publishing NodeStatus once to trigger querying
00217      * Querying for 2 seconds, no responses will be sent (there's no server)
00218      */
00219     handler.can_discover = true;
00220 
00221     uavcan::Publisher<uavcan::protocol::NodeStatus> node_status_pub(nodes.b);
00222     ASSERT_LE(0, node_status_pub.init());
00223 
00224     uavcan::protocol::NodeStatus node_status;
00225     node_status.uptime_sec = 10;                        // Nonzero
00226     ASSERT_LE(0, node_status_pub.broadcast(node_status));
00227 
00228     nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(3100));
00229 
00230     ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryNewNodeFound));
00231     ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryTimerStart));
00232     ASSERT_EQ(0, tracer.countEvents(TraceDiscoveryTimerStop));
00233     ASSERT_EQ(4, tracer.countEvents(TraceDiscoveryGetNodeInfoRequest));
00234     ASSERT_EQ(3, tracer.countEvents(TraceDiscoveryGetNodeInfoFailure));
00235     ASSERT_EQ(0, tracer.countEvents(TraceDiscoveryNodeFinalized));
00236     ASSERT_EQ(0, tracer.countEvents(TraceDiscoveryNodeRestartDetected));
00237     ASSERT_TRUE(disc.hasUnknownNodes());
00238 
00239     /*
00240      * Emulating node restart
00241      */
00242     node_status.uptime_sec = 9;                         // Less than previous
00243     ASSERT_LE(0, node_status_pub.broadcast(node_status));
00244 
00245     nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(3100));
00246 
00247     ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryNewNodeFound));
00248     ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryTimerStart));
00249     ASSERT_EQ(0, tracer.countEvents(TraceDiscoveryTimerStop));
00250     ASSERT_EQ(7, tracer.countEvents(TraceDiscoveryGetNodeInfoRequest));
00251     ASSERT_EQ(6, tracer.countEvents(TraceDiscoveryGetNodeInfoFailure));
00252     ASSERT_EQ(0, tracer.countEvents(TraceDiscoveryNodeFinalized));
00253     ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryNodeRestartDetected));
00254     ASSERT_TRUE(disc.hasUnknownNodes());
00255 
00256     /*
00257      * Waiting for timeout
00258      */
00259     nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(3100));
00260 
00261     ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryNewNodeFound));
00262     ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryTimerStart));
00263     ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryTimerStop));
00264     ASSERT_EQ(8, tracer.countEvents(TraceDiscoveryGetNodeInfoRequest));
00265     ASSERT_EQ(8, tracer.countEvents(TraceDiscoveryGetNodeInfoFailure));
00266     ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryNodeFinalized));
00267     ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryNodeRestartDetected));
00268     ASSERT_FALSE(disc.hasUnknownNodes());
00269 
00270     /*
00271      * Checking the results
00272      */
00273     ASSERT_TRUE(handler.findNode(UniqueID()));
00274     ASSERT_EQ(2, handler.findNode(UniqueID())->node_id.get());
00275 }
00276 
00277 
00278 TEST(dynamic_node_id_server_NodeDiscoverer, Sizes)
00279 {
00280     using namespace uavcan;
00281 
00282     std::cout << "BitSet<NodeID::Max + 1>:              " << sizeof(BitSet<NodeID::Max + 1>) << std::endl;
00283     std::cout << "ServiceClient<protocol::GetNodeInfo>: " << sizeof(ServiceClient<protocol::GetNodeInfo>) << std::endl;
00284     std::cout << "protocol::GetNodeInfo::Response:      " << sizeof(protocol::GetNodeInfo::Response) << std::endl;
00285     std::cout << "Subscriber<protocol::NodeStatus>:     " << sizeof(Subscriber<protocol::NodeStatus>) << std::endl;
00286 }