dispatcher.cpp

00001 /*
00002  * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
00003  */
00004 
00005 #include <memory>
00006 #include <vector>
00007 #include <gtest/gtest.h>
00008 #include "transfer_test_helpers.hpp"
00009 #include "can/can.hpp"
00010 #include <uavcan/transport/dispatcher.hpp>
00011 
00012 
00013 class DispatcherTransferEmulator : public IncomingTransferEmulatorBase
00014 {
00015     CanDriverMock& target_;
00016 
00017 public:
00018     DispatcherTransferEmulator(CanDriverMock& target, uavcan::NodeID dst_node_id = 127)
00019         : IncomingTransferEmulatorBase(dst_node_id)
00020         , target_(target)
00021     { }
00022 
00023     void sendOneFrame(const uavcan::RxFrame& frame)
00024     {
00025         CanIfaceMock* const iface = static_cast<CanIfaceMock*>(target_.getIface(frame.getIfaceIndex()));
00026         EXPECT_TRUE(iface);
00027         if (iface)
00028         {
00029             iface->pushRx(frame);
00030         }
00031     }
00032 };
00033 
00034 
00035 struct RxFrameListener : public uavcan::IRxFrameListener
00036 {
00037     std::vector<uavcan::CanRxFrame> rx_frames;
00038 
00039     virtual void handleRxFrame(const uavcan::CanRxFrame& frame, uavcan::CanIOFlags flags)
00040     {
00041         std::cout << "RX frame [flags=" << flags << "]: " << frame.toString() << std::endl;
00042         if ((flags & uavcan::CanIOFlagLoopback) == 0)
00043         {
00044             rx_frames.push_back(frame);
00045         }
00046     }
00047 };
00048 
00049 
00050 static const uavcan::NodeID SELF_NODE_ID(64);
00051 
00052 
00053 TEST(Dispatcher, Reception)
00054 {
00055     uavcan::PoolAllocator<uavcan::MemPoolBlockSize * 100, uavcan::MemPoolBlockSize>  pool;
00056 
00057     SystemClockMock clockmock(100);
00058     CanDriverMock driver(2, clockmock);
00059 
00060     uavcan::Dispatcher dispatcher(driver, pool, clockmock);
00061     ASSERT_TRUE(dispatcher.setNodeID(SELF_NODE_ID));  // Can be set only once
00062     ASSERT_FALSE(dispatcher.setNodeID(SELF_NODE_ID));
00063     ASSERT_EQ(SELF_NODE_ID, dispatcher.getNodeID());
00064 
00065     DispatcherTransferEmulator emulator(driver, SELF_NODE_ID);
00066 
00067     /*
00068      * RX listener
00069      */
00070     RxFrameListener rx_listener;
00071     ASSERT_FALSE(dispatcher.getRxFrameListener());
00072     dispatcher.installRxFrameListener(&rx_listener);
00073     ASSERT_TRUE(dispatcher.getRxFrameListener());
00074     ASSERT_TRUE(rx_listener.rx_frames.empty());
00075 
00076     /*
00077      * Test environment
00078      */
00079     static const uavcan::DataTypeDescriptor TYPES[4] =
00080     {
00081         makeDataType(uavcan::DataTypeKindMessage, 1),
00082         makeDataType(uavcan::DataTypeKindMessage, 2),
00083         makeDataType(uavcan::DataTypeKindService, 1),
00084         makeDataType(uavcan::DataTypeKindService, 1)
00085     };
00086 
00087     typedef std::auto_ptr<TestListener> TestListenerPtr;
00088     static const int MaxBufSize = 512;
00089     static const int NumSubscribers = 6;
00090     TestListenerPtr subscribers[NumSubscribers] =
00091     {
00092         TestListenerPtr(new TestListener(dispatcher.getTransferPerfCounter(), TYPES[0], MaxBufSize, pool)), // msg
00093         TestListenerPtr(new TestListener(dispatcher.getTransferPerfCounter(), TYPES[0], MaxBufSize, pool)), // msg // Two similar
00094         TestListenerPtr(new TestListener(dispatcher.getTransferPerfCounter(), TYPES[1], MaxBufSize, pool)), // msg
00095         TestListenerPtr(new TestListener(dispatcher.getTransferPerfCounter(), TYPES[2], MaxBufSize, pool)), // srv
00096         TestListenerPtr(new TestListener(dispatcher.getTransferPerfCounter(), TYPES[3], MaxBufSize, pool)), // srv
00097         TestListenerPtr(new TestListener(dispatcher.getTransferPerfCounter(), TYPES[3], MaxBufSize, pool))  // srv // Repeat again
00098     };
00099 
00100     static const std::string DATA[6] =
00101     {
00102         "Yes, man is mortal, but that would be only half the trouble. "
00103         "The worst of it is that he's sometimes unexpectedly mortal - there's the trick!",
00104 
00105         "In fact, I'm beginning to fear that this confusion will go on for a long time. "
00106         "And all because he writes down what I said incorrectly.",
00107 
00108         "I had the pleasure of meeting that young man at the Patriarch's Ponds. "
00109         "He almost drove me mad myself, proving to me that I don't exist. But you do believe that it is really I?",
00110 
00111         "He was a dreamer, a thinker, a speculative philosopher... or, as his wife would have it, an idiot.",
00112 
00113         "The only way to get ideas for stories is to drink way too much coffee and buy a desk that doesn't "
00114         "collapse when you beat your head against it",
00115 
00116         ""
00117     };
00118 
00119     for (unsigned i = 0; i < sizeof(DATA) / sizeof(DATA[0]); i++)
00120     {
00121         std::cout << "Size of test data chunk " << i << ": " << DATA[i].length() << std::endl;
00122     }
00123 
00124     const Transfer transfers[8] =
00125     {
00126         emulator.makeTransfer(0,  uavcan::TransferTypeMessageBroadcast, 10, DATA[0], TYPES[0]),
00127         emulator.makeTransfer(5,  uavcan::TransferTypeMessageBroadcast, 11, DATA[1], TYPES[1]),
00128         emulator.makeTransfer(10, uavcan::TransferTypeServiceRequest,   12, DATA[2], TYPES[2]),
00129         emulator.makeTransfer(15, uavcan::TransferTypeServiceResponse,  13, DATA[4], TYPES[3]),
00130         emulator.makeTransfer(20, uavcan::TransferTypeMessageBroadcast, 14, DATA[3], TYPES[0]),
00131         emulator.makeTransfer(25, uavcan::TransferTypeMessageBroadcast, 15, DATA[5], TYPES[1]),
00132         // Wrongly addressed:
00133         emulator.makeTransfer(29, uavcan::TransferTypeServiceResponse,  10, DATA[0], TYPES[3], 100),
00134         emulator.makeTransfer(31, uavcan::TransferTypeServiceRequest,   11, DATA[4], TYPES[2], 101)
00135     };
00136 
00137     /*
00138      * Registration
00139      */
00140     for (int i = 0; i < NumSubscribers; i++)
00141     {
00142         ASSERT_FALSE(dispatcher.hasSubscriber(subscribers[i]->getDataTypeDescriptor().getID()));
00143         ASSERT_FALSE(dispatcher.hasPublisher(subscribers[i]->getDataTypeDescriptor().getID()));
00144         ASSERT_FALSE(dispatcher.hasServer(subscribers[i]->getDataTypeDescriptor().getID()));
00145     }
00146 
00147     ASSERT_TRUE(dispatcher.registerMessageListener(subscribers[0].get()));
00148     ASSERT_TRUE(dispatcher.registerMessageListener(subscribers[1].get()));
00149     ASSERT_TRUE(dispatcher.registerMessageListener(subscribers[2].get()));
00150     ASSERT_TRUE(dispatcher.registerServiceRequestListener(subscribers[3].get()));
00151     ASSERT_TRUE(dispatcher.registerServiceResponseListener(subscribers[4].get()));
00152     ASSERT_TRUE(dispatcher.registerServiceResponseListener(subscribers[5].get()));
00153 
00154     for (int i = 0; i < NumSubscribers; i++)
00155     {
00156         ASSERT_FALSE(dispatcher.hasPublisher(subscribers[i]->getDataTypeDescriptor().getID()));
00157     }
00158 
00159     // Subscribers
00160     ASSERT_TRUE(dispatcher.hasSubscriber(subscribers[0]->getDataTypeDescriptor().getID()));
00161     ASSERT_TRUE(dispatcher.hasSubscriber(subscribers[1]->getDataTypeDescriptor().getID()));
00162     ASSERT_TRUE(dispatcher.hasSubscriber(subscribers[2]->getDataTypeDescriptor().getID()));
00163 
00164     // Servers
00165     ASSERT_TRUE(dispatcher.hasServer(subscribers[3]->getDataTypeDescriptor().getID()));
00166 
00167     /*
00168      * Sending the transfers
00169      */
00170     // Multiple service request listeners are not allowed
00171     ASSERT_FALSE(dispatcher.registerServiceRequestListener(subscribers[3].get()));
00172 
00173     // Item count validation
00174     ASSERT_EQ(3, dispatcher.getNumMessageListeners());
00175     ASSERT_EQ(1, dispatcher.getNumServiceRequestListeners());
00176     ASSERT_EQ(2, dispatcher.getNumServiceResponseListeners());
00177 
00178     for (int i = 0; i < NumSubscribers; i++)
00179     {
00180         ASSERT_TRUE(subscribers[i]->isEmpty());
00181     }
00182 
00183     emulator.send(transfers);
00184     emulator.send(transfers);  // Just for fun, they will be ignored anyway.
00185 
00186     while (true)
00187     {
00188         const int res = dispatcher.spinOnce();
00189         ASSERT_LE(0, res);
00190         clockmock.advance(100);
00191         if (res == 0)
00192         {
00193             break;
00194         }
00195     }
00196 
00197     /*
00198      * Matching.
00199      */
00200     ASSERT_TRUE(subscribers[0]->matchAndPop(transfers[4]));
00201     ASSERT_TRUE(subscribers[0]->matchAndPop(transfers[0]));
00202 
00203     ASSERT_TRUE(subscribers[1]->matchAndPop(transfers[4]));
00204     ASSERT_TRUE(subscribers[1]->matchAndPop(transfers[0]));
00205 
00206     ASSERT_TRUE(subscribers[2]->matchAndPop(transfers[5]));
00207     ASSERT_TRUE(subscribers[2]->matchAndPop(transfers[1]));
00208 
00209     ASSERT_TRUE(subscribers[3]->matchAndPop(transfers[2]));
00210 
00211     ASSERT_TRUE(subscribers[4]->matchAndPop(transfers[3]));
00212 
00213     ASSERT_TRUE(subscribers[5]->matchAndPop(transfers[3]));
00214 
00215     for (int i = 0; i < NumSubscribers; i++)
00216     {
00217         ASSERT_TRUE(subscribers[i]->isEmpty());
00218     }
00219 
00220     /*
00221      * Unregistering all transfers
00222      */
00223     dispatcher.unregisterMessageListener(subscribers[0].get());
00224     dispatcher.unregisterMessageListener(subscribers[1].get());
00225     dispatcher.unregisterMessageListener(subscribers[2].get());
00226     dispatcher.unregisterServiceRequestListener(subscribers[3].get());
00227     dispatcher.unregisterServiceResponseListener(subscribers[4].get());
00228     dispatcher.unregisterServiceResponseListener(subscribers[5].get());
00229 
00230     ASSERT_EQ(0, dispatcher.getNumMessageListeners());
00231     ASSERT_EQ(0, dispatcher.getNumServiceRequestListeners());
00232     ASSERT_EQ(0, dispatcher.getNumServiceResponseListeners());
00233 
00234     /*
00235      * Perf counters
00236      */
00237     EXPECT_LT(0, dispatcher.getTransferPerfCounter().getErrorCount());   // Repeated transfers
00238     EXPECT_EQ(0, dispatcher.getTransferPerfCounter().getTxTransferCount());
00239     EXPECT_EQ(9, dispatcher.getTransferPerfCounter().getRxTransferCount());
00240 
00241     /*
00242      * RX listener
00243      */
00244     std::cout << "Num received frames: " << rx_listener.rx_frames.size() << std::endl;
00245     ASSERT_EQ(292, rx_listener.rx_frames.size());
00246 }
00247 
00248 
00249 TEST(Dispatcher, Transmission)
00250 {
00251     uavcan::PoolAllocator<uavcan::MemPoolBlockSize * 8, uavcan::MemPoolBlockSize> pool;
00252 
00253     SystemClockMock clockmock(100);
00254     CanDriverMock driver(2, clockmock);
00255 
00256     uavcan::Dispatcher dispatcher(driver, pool, clockmock);
00257     ASSERT_TRUE(dispatcher.setNodeID(SELF_NODE_ID));  // Can be set only once
00258     ASSERT_FALSE(dispatcher.setNodeID(SELF_NODE_ID));
00259 
00260     /*
00261      * RX listener
00262      */
00263     RxFrameListener rx_listener;
00264     dispatcher.installRxFrameListener(&rx_listener);
00265 
00266     /*
00267      * Transmission
00268      */
00269     static const uavcan::MonotonicTime TX_DEADLINE = tsMono(123456);
00270 
00271     // uint_fast16_t data_type_id, TransferType transfer_type, NodeID src_node_id, NodeID dst_node_id,
00272     // uint_fast8_t frame_index, TransferID transfer_id, bool last_frame = false
00273     uavcan::Frame frame(123, uavcan::TransferTypeServiceRequest, SELF_NODE_ID, 2, 0);
00274     frame.setPayload(reinterpret_cast<const uint8_t*>("123"), 3);
00275 
00276     ASSERT_FALSE(dispatcher.hasPublisher(123));
00277     ASSERT_FALSE(dispatcher.hasPublisher(456));
00278     const uavcan::OutgoingTransferRegistryKey otr_key(123, uavcan::TransferTypeMessageBroadcast, 0);
00279     ASSERT_TRUE(dispatcher.getOutgoingTransferRegistry().accessOrCreate(otr_key,
00280                                                                         uavcan::MonotonicTime::fromMSec(1000000)));
00281     ASSERT_TRUE(dispatcher.hasPublisher(123));
00282     ASSERT_FALSE(dispatcher.hasPublisher(456));
00283 
00284     ASSERT_EQ(2, dispatcher.send(frame, TX_DEADLINE, tsMono(0), uavcan::CanTxQueue::Volatile, 0, 0xFF));
00285 
00286     /*
00287      * Validation
00288      */
00289     uavcan::CanFrame expected_can_frame;
00290     ASSERT_TRUE(frame.compile(expected_can_frame));
00291 
00292     ASSERT_TRUE(driver.ifaces.at(0).matchAndPopTx(expected_can_frame, TX_DEADLINE));
00293     ASSERT_TRUE(driver.ifaces.at(1).matchAndPopTx(expected_can_frame, TX_DEADLINE));
00294 
00295     ASSERT_TRUE(driver.ifaces.at(0).tx.empty());
00296     ASSERT_TRUE(driver.ifaces.at(1).tx.empty());
00297 
00298     /*
00299      * Perf counters - all empty because dispatcher itself does not count TX transfers
00300      */
00301     EXPECT_EQ(0, dispatcher.getTransferPerfCounter().getErrorCount());
00302     EXPECT_EQ(0, dispatcher.getTransferPerfCounter().getTxTransferCount());
00303     EXPECT_EQ(0, dispatcher.getTransferPerfCounter().getRxTransferCount());
00304 
00305     /*
00306      * RX listener
00307      */
00308     ASSERT_TRUE(rx_listener.rx_frames.empty());
00309 }
00310 
00311 
00312 TEST(Dispatcher, Spin)
00313 {
00314     NullAllocator poolmgr;
00315 
00316     SystemClockMock clockmock(100);
00317     CanDriverMock driver(2, clockmock);
00318 
00319     uavcan::Dispatcher dispatcher(driver, poolmgr, clockmock);
00320     ASSERT_TRUE(dispatcher.setNodeID(SELF_NODE_ID));  // Can be set only once
00321     ASSERT_FALSE(dispatcher.setNodeID(SELF_NODE_ID));
00322 
00323     clockmock.monotonic_auto_advance = 100;
00324 
00325     ASSERT_EQ(100, clockmock.monotonic);
00326     ASSERT_EQ(0, dispatcher.spin(tsMono(1000)));
00327     ASSERT_LE(1000, clockmock.monotonic);
00328     ASSERT_EQ(0, dispatcher.spinOnce());
00329     ASSERT_LE(1000, clockmock.monotonic);
00330     ASSERT_EQ(0, dispatcher.spin(tsMono(1100)));
00331     ASSERT_LE(1100, clockmock.monotonic);
00332 }
00333 
00334 
00335 struct DispatcherTestLoopbackFrameListener : public uavcan::LoopbackFrameListenerBase
00336 {
00337     uavcan::RxFrame last_frame;
00338     unsigned count;
00339 
00340     DispatcherTestLoopbackFrameListener(uavcan::Dispatcher& dispatcher)
00341         : uavcan::LoopbackFrameListenerBase(dispatcher)
00342         , count(0)
00343     { }
00344 
00345     using uavcan::LoopbackFrameListenerBase::startListening;
00346     using uavcan::LoopbackFrameListenerBase::isListening;
00347 
00348     void handleLoopbackFrame(const uavcan::RxFrame& frame)
00349     {
00350         std::cout << "DispatcherTestLoopbackFrameListener: " << frame.toString() << std::endl;
00351         last_frame = frame;
00352         count++;
00353     }
00354 };
00355 
00356 TEST(Dispatcher, Loopback)
00357 {
00358     NullAllocator poolmgr;
00359 
00360     SystemClockMock clockmock(100);
00361     CanDriverMock driver(2, clockmock);
00362 
00363     uavcan::Dispatcher dispatcher(driver, poolmgr, clockmock);
00364     ASSERT_TRUE(dispatcher.setNodeID(SELF_NODE_ID));
00365 
00366     {
00367         DispatcherTestLoopbackFrameListener listener(dispatcher);
00368         ASSERT_FALSE(listener.isListening());
00369         listener.startListening();
00370         ASSERT_TRUE(listener.isListening());
00371 
00372         // uint_fast16_t data_type_id, TransferType transfer_type, NodeID src_node_id, NodeID dst_node_id,
00373         // uint_fast8_t frame_index, TransferID transfer_id, bool last_frame = false
00374         uavcan::Frame frame(123, uavcan::TransferTypeServiceResponse, SELF_NODE_ID, 2, 0);
00375         frame.setPayload(reinterpret_cast<const uint8_t*>("123"), 3);
00376 
00377         ASSERT_TRUE(listener.last_frame == uavcan::RxFrame());
00378 
00379         ASSERT_LE(0, dispatcher.send(frame, tsMono(1000), tsMono(0), uavcan::CanTxQueue::Persistent,
00380                                      uavcan::CanIOFlagLoopback, 0xFF));
00381 
00382         ASSERT_EQ(0, dispatcher.spin(tsMono(1000)));
00383 
00384         ASSERT_TRUE(listener.last_frame != uavcan::RxFrame());
00385         ASSERT_TRUE(listener.last_frame == frame);
00386         ASSERT_EQ(1, listener.last_frame.getIfaceIndex());  // Last iface
00387         ASSERT_EQ(2, listener.count);
00388 
00389         ASSERT_EQ(1, dispatcher.getLoopbackFrameListenerRegistry().getNumListeners());
00390     }
00391     ASSERT_EQ(0, dispatcher.getLoopbackFrameListenerRegistry().getNumListeners());
00392 }