dynamic_node_id_client.cpp

00001 /*
00002  * Copyright (C) 2015 Pavel Kirienko <pavel.kirienko@gmail.com>
00003  */
00004 
00005 #include <gtest/gtest.h>
00006 #include <uavcan/protocol/dynamic_node_id_client.hpp>
00007 #include "helpers.hpp"
00008 
00009 
00010 TEST(DynamicNodeIDClient, Basic)
00011 {
00012     // Node A is Allocator, Node B is Allocatee
00013     InterlinkedTestNodesWithSysClock nodes(uavcan::NodeID(10), uavcan::NodeID::Broadcast);
00014 
00015     uavcan::DynamicNodeIDClient dnidac(nodes.b);
00016 
00017     uavcan::GlobalDataTypeRegistry::instance().reset();
00018     uavcan::DefaultDataTypeRegistrator<uavcan::protocol::dynamic_node_id::Allocation> _reg1;
00019     (void)_reg1;
00020 
00021     /*
00022      * Client initialization
00023      */
00024     uavcan::protocol::HardwareVersion::FieldTypes::unique_id unique_id;
00025 
00026     ASSERT_LE(-uavcan::ErrInvalidParam, dnidac.start(unique_id));  // Empty hardware version is not allowed
00027 
00028     for (uavcan::uint8_t i = 0; i < unique_id.size(); i++)
00029     {
00030         unique_id[i] = i;
00031     }
00032 
00033     ASSERT_LE(-uavcan::ErrInvalidParam, dnidac.start(unique_id, uavcan::NodeID()));
00034 
00035     const uavcan::NodeID PreferredNodeID = 42;
00036     ASSERT_LE(0, dnidac.start(unique_id, PreferredNodeID));
00037 
00038     ASSERT_FALSE(dnidac.getAllocatedNodeID().isValid());
00039     ASSERT_FALSE(dnidac.getAllocatorNodeID().isValid());
00040     ASSERT_FALSE(dnidac.isAllocationComplete());
00041 
00042     /*
00043      * Subscriber (server emulation)
00044      */
00045     SubscriberWithCollector<uavcan::protocol::dynamic_node_id::Allocation> dynid_sub(nodes.a);
00046     ASSERT_LE(0, dynid_sub.start());
00047     dynid_sub.subscriber.allowAnonymousTransfers();
00048 
00049     /*
00050      * Monitoring requests
00051      */
00052     nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1500));
00053     ASSERT_TRUE(dynid_sub.collector.msg.get());
00054     std::cout << "First-stage request:\n" << *dynid_sub.collector.msg << std::endl;
00055     ASSERT_EQ(PreferredNodeID.get(), dynid_sub.collector.msg->node_id);
00056     ASSERT_TRUE(dynid_sub.collector.msg->first_part_of_unique_id);
00057     ASSERT_TRUE(uavcan::equal(dynid_sub.collector.msg->unique_id.begin(),
00058                               dynid_sub.collector.msg->unique_id.end(),
00059                               unique_id.begin()));
00060     dynid_sub.collector.msg.reset();
00061 
00062     // Second - rate is no lower than 0.5 Hz
00063     nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1500));
00064     ASSERT_TRUE(dynid_sub.collector.msg.get());
00065     dynid_sub.collector.msg.reset();
00066 
00067     ASSERT_FALSE(dnidac.getAllocatedNodeID().isValid());
00068     ASSERT_FALSE(dnidac.getAllocatorNodeID().isValid());
00069     ASSERT_FALSE(dnidac.isAllocationComplete());
00070 
00071     /*
00072      * Publisher (server emulation)
00073      */
00074     uavcan::Publisher<uavcan::protocol::dynamic_node_id::Allocation> dynid_pub(nodes.a);
00075     ASSERT_LE(0, dynid_pub.init());
00076 
00077     /*
00078      * Sending some some Allocation messages - the timer will keep restarting
00079      */
00080     for (int i = 0; i < 10; i++)
00081     {
00082         uavcan::protocol::dynamic_node_id::Allocation msg;  // Contents of the message doesn't matter
00083         ASSERT_LE(0, dynid_pub.broadcast(msg));
00084         nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(210));
00085         ASSERT_FALSE(dynid_sub.collector.msg.get());
00086     }
00087 
00088     /*
00089      * Responding with partially matching unique ID - the client will respond with second-stage request immediately
00090      */
00091     const uint8_t BytesPerRequest = uavcan::protocol::dynamic_node_id::Allocation::MAX_LENGTH_OF_UNIQUE_ID_IN_REQUEST;
00092     {
00093         uavcan::protocol::dynamic_node_id::Allocation msg;
00094         msg.unique_id.resize(BytesPerRequest);
00095         uavcan::copy(unique_id.begin(), unique_id.begin() + BytesPerRequest, msg.unique_id.begin());
00096 
00097         std::cout << "First-stage offer:\n" << msg << std::endl;
00098 
00099         ASSERT_FALSE(dynid_sub.collector.msg.get());
00100         ASSERT_LE(0, dynid_pub.broadcast(msg));
00101         nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(500));
00102 
00103         ASSERT_TRUE(dynid_sub.collector.msg.get());
00104         std::cout << "Second-stage request:\n" << *dynid_sub.collector.msg << std::endl;
00105         ASSERT_EQ(PreferredNodeID.get(), dynid_sub.collector.msg->node_id);
00106         ASSERT_FALSE(dynid_sub.collector.msg->first_part_of_unique_id);
00107         ASSERT_TRUE(uavcan::equal(dynid_sub.collector.msg->unique_id.begin(),
00108                                   dynid_sub.collector.msg->unique_id.end(),
00109                                   unique_id.begin() + BytesPerRequest));
00110         dynid_sub.collector.msg.reset();
00111     }
00112 
00113     /*
00114      * Responding with second-stage offer, expecting the last request back
00115      */
00116     {
00117         uavcan::protocol::dynamic_node_id::Allocation msg;
00118         msg.unique_id.resize(BytesPerRequest * 2);
00119         uavcan::copy(unique_id.begin(), unique_id.begin() + BytesPerRequest * 2, msg.unique_id.begin());
00120 
00121         std::cout << "Second-stage offer:\n" << msg << std::endl;
00122 
00123         ASSERT_FALSE(dynid_sub.collector.msg.get());
00124         ASSERT_LE(0, dynid_pub.broadcast(msg));
00125         nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(500));
00126 
00127         ASSERT_TRUE(dynid_sub.collector.msg.get());
00128         std::cout << "Last request:\n" << *dynid_sub.collector.msg << std::endl;
00129         ASSERT_EQ(PreferredNodeID.get(), dynid_sub.collector.msg->node_id);
00130         ASSERT_FALSE(dynid_sub.collector.msg->first_part_of_unique_id);
00131         ASSERT_TRUE(uavcan::equal(dynid_sub.collector.msg->unique_id.begin(),
00132                                   dynid_sub.collector.msg->unique_id.end(),
00133                                   unique_id.begin() + BytesPerRequest * 2));
00134         dynid_sub.collector.msg.reset();
00135     }
00136 
00137     ASSERT_FALSE(dnidac.getAllocatedNodeID().isValid());
00138     ASSERT_FALSE(dnidac.getAllocatorNodeID().isValid());
00139     ASSERT_FALSE(dnidac.isAllocationComplete());
00140 
00141     /*
00142      * Now we have full unique ID for this client received, and it is possible to grant allocation
00143      */
00144     {
00145         uavcan::protocol::dynamic_node_id::Allocation msg;
00146         msg.unique_id.resize(16);
00147         msg.node_id = 72;
00148         uavcan::copy(unique_id.begin(), unique_id.end(), msg.unique_id.begin());
00149 
00150         ASSERT_FALSE(dynid_sub.collector.msg.get());
00151         ASSERT_LE(0, dynid_pub.broadcast(msg));
00152         nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(2000));
00153         ASSERT_FALSE(dynid_sub.collector.msg.get());
00154     }
00155 
00156     ASSERT_EQ(uavcan::NodeID(72), dnidac.getAllocatedNodeID());
00157     ASSERT_EQ(uavcan::NodeID(10), dnidac.getAllocatorNodeID());
00158     ASSERT_TRUE(dnidac.isAllocationComplete());
00159 }
00160 
00161 
00162 TEST(DynamicNodeIDClient, NonPassiveMode)
00163 {
00164     InterlinkedTestNodesWithSysClock nodes;
00165 
00166     uavcan::DynamicNodeIDClient dnidac(nodes.b);
00167 
00168     uavcan::GlobalDataTypeRegistry::instance().reset();
00169     uavcan::DefaultDataTypeRegistrator<uavcan::protocol::dynamic_node_id::Allocation> _reg1;
00170     (void)_reg1;
00171 
00172     uavcan::protocol::HardwareVersion::FieldTypes::unique_id unique_id;
00173     for (uavcan::uint8_t i = 0; i < unique_id.size(); i++)
00174     {
00175         unique_id[i] = i;
00176     }
00177 
00178     ASSERT_LE(-uavcan::ErrLogic, dnidac.start(unique_id));
00179 }