test_node.hpp

00001 /*
00002  * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
00003  */
00004 
00005 #pragma once
00006 
00007 #if __GNUC__
00008 // We need auto_ptr for compatibility reasons
00009 # pragma GCC diagnostic ignored "-Wdeprecated-declarations"
00010 # pragma GCC diagnostic ignored "-Wzero-as-null-pointer-constant"
00011 #endif
00012 
00013 #include <uavcan/node/abstract_node.hpp>
00014 #include <uavcan/helpers/heap_based_pool_allocator.hpp>
00015 #include <memory>
00016 #include <set>
00017 #include <queue>
00018 #include "../transport/can/can.hpp"
00019 #include <uavcan/util/method_binder.hpp>
00020 #include <uavcan/node/subscriber.hpp>
00021 
00022 struct TestNode : public uavcan::INode
00023 {
00024     /*
00025      * This class used to use the simple pool allocator instead:
00026      *  uavcan::PoolAllocator<uavcan::MemPoolBlockSize * 1024, uavcan::MemPoolBlockSize> pool;
00027      * It has been replaced because unlike the simple allocator, heap-based one is not tested as extensively.
00028      * Moreover, heap based allocator prints and error message upon destruction if some memory has not been freed.
00029      */
00030     uavcan::HeapBasedPoolAllocator<uavcan::MemPoolBlockSize>  pool;
00031     uavcan::Scheduler scheduler;
00032     uint64_t internal_failure_count;
00033 
00034     TestNode(uavcan::ICanDriver& can_driver, uavcan::ISystemClock& clock_driver, uavcan::NodeID self_node_id) :
00035         pool(1024),
00036         scheduler(can_driver, pool, clock_driver),
00037         internal_failure_count(0)
00038     {
00039         setNodeID(self_node_id);
00040     }
00041 
00042     virtual void registerInternalFailure(const char* msg)
00043     {
00044         std::cout << "TestNode internal failure: " << msg << std::endl;
00045         internal_failure_count++;
00046     }
00047 
00048     virtual uavcan::IPoolAllocator& getAllocator() { return pool; }
00049     virtual uavcan::Scheduler& getScheduler() { return scheduler; }
00050     virtual const uavcan::Scheduler& getScheduler() const { return scheduler; }
00051 };
00052 
00053 
00054 struct PairableCanDriver : public uavcan::ICanDriver, public uavcan::ICanIface
00055 {
00056     uavcan::ISystemClock& clock;
00057     std::set<PairableCanDriver*> others;
00058     std::queue<uavcan::CanFrame> read_queue;
00059     std::queue<uavcan::CanFrame> loopback_queue;
00060     uint64_t error_count;
00061 
00062     PairableCanDriver(uavcan::ISystemClock& clock)
00063         : clock(clock)
00064         , error_count(0)
00065     { }
00066 
00067     void linkTogether(PairableCanDriver* with)
00068     {
00069         this->others.insert(with);
00070         with->others.insert(this);
00071         others.erase(this);
00072     }
00073 
00074     virtual uavcan::ICanIface* getIface(uavcan::uint8_t iface_index)
00075     {
00076         return (iface_index == 0) ? this : UAVCAN_NULLPTR;
00077     }
00078     virtual const uavcan::ICanIface* getIface(uavcan::uint8_t iface_index) const
00079     {
00080         return (iface_index == 0) ? this : UAVCAN_NULLPTR;
00081     }
00082 
00083     virtual uavcan::uint8_t getNumIfaces() const { return 1; }
00084 
00085     virtual uavcan::int16_t select(uavcan::CanSelectMasks& inout_masks,
00086                                    const uavcan::CanFrame* (&)[uavcan::MaxCanIfaces],
00087                                    uavcan::MonotonicTime blocking_deadline)
00088     {
00089         if (inout_masks.read == 1)
00090         {
00091             inout_masks.read = (!read_queue.empty() || !loopback_queue.empty()) ? 1 : 0;
00092         }
00093         if (inout_masks.read || inout_masks.write)
00094         {
00095             return 1;
00096         }
00097         while (clock.getMonotonic() < blocking_deadline)
00098         {
00099             usleep(1000);
00100         }
00101         return 0;
00102     }
00103 
00104     virtual uavcan::int16_t send(const uavcan::CanFrame& frame, uavcan::MonotonicTime, uavcan::CanIOFlags flags)
00105     {
00106         assert(!others.empty());
00107         for (std::set<PairableCanDriver*>::iterator it = others.begin(); it != others.end(); ++it)
00108         {
00109             (*it)->read_queue.push(frame);
00110         }
00111         if (flags & uavcan::CanIOFlagLoopback)
00112         {
00113             loopback_queue.push(frame);
00114         }
00115         return 1;
00116     }
00117 
00118     virtual uavcan::int16_t receive(uavcan::CanFrame& out_frame, uavcan::MonotonicTime& out_ts_monotonic,
00119                                     uavcan::UtcTime& out_ts_utc, uavcan::CanIOFlags& out_flags)
00120     {
00121         out_flags = 0;
00122         if (loopback_queue.empty())
00123         {
00124             assert(read_queue.size());
00125             out_frame = read_queue.front();
00126             read_queue.pop();
00127         }
00128         else
00129         {
00130             out_flags |= uavcan::CanIOFlagLoopback;
00131             out_frame = loopback_queue.front();
00132             loopback_queue.pop();
00133         }
00134         out_ts_monotonic = clock.getMonotonic();
00135         out_ts_utc = clock.getUtc();
00136         return 1;
00137     }
00138 
00139     void pushRxToAllIfaces(const uavcan::CanFrame& can_frame)
00140     {
00141         read_queue.push(can_frame);
00142     }
00143 
00144     virtual uavcan::int16_t configureFilters(const uavcan::CanFilterConfig*, uavcan::uint16_t) { return -1; }
00145     virtual uavcan::uint16_t getNumFilters() const { return 0; }
00146     virtual uavcan::uint64_t getErrorCount() const { return error_count; }
00147 };
00148 
00149 
00150 template <typename ClockType>
00151 struct InterlinkedTestNodes
00152 {
00153     ClockType clock_a;
00154     ClockType clock_b;
00155     PairableCanDriver can_a;
00156     PairableCanDriver can_b;
00157     TestNode a;
00158     TestNode b;
00159 
00160     InterlinkedTestNodes(uavcan::NodeID nid_first, uavcan::NodeID nid_second)
00161         : can_a(clock_a)
00162         , can_b(clock_b)
00163         , a(can_a, clock_a, nid_first)
00164         , b(can_b, clock_b, nid_second)
00165     {
00166         can_a.linkTogether(&can_b);
00167     }
00168 
00169     InterlinkedTestNodes()
00170         : can_a(clock_a)
00171         , can_b(clock_b)
00172         , a(can_a, clock_a, 1)
00173         , b(can_b, clock_b, 2)
00174     {
00175         can_a.linkTogether(&can_b);
00176     }
00177 
00178     int spinBoth(uavcan::MonotonicDuration duration)
00179     {
00180         assert(!duration.isNegative());
00181         unsigned nspins2 = unsigned(duration.toMSec() / 2);
00182         nspins2 = nspins2 ? nspins2 : 1;
00183         while (nspins2 --> 0)
00184         {
00185             int ret = a.spin(uavcan::MonotonicDuration::fromMSec(1));
00186             if (ret < 0)
00187             {
00188                 return ret;
00189             }
00190             ret = b.spin(uavcan::MonotonicDuration::fromMSec(1));
00191             if (ret < 0)
00192             {
00193                 return ret;
00194             }
00195         }
00196         return 0;
00197     }
00198 };
00199 
00200 
00201 typedef InterlinkedTestNodes<SystemClockDriver> InterlinkedTestNodesWithSysClock;
00202 typedef InterlinkedTestNodes<SystemClockMock> InterlinkedTestNodesWithClockMock;
00203 
00204 
00205 template <unsigned NumNodes>
00206 struct TestNetwork
00207 {
00208     struct NodeEnvironment
00209     {
00210         SystemClockDriver clock;
00211         PairableCanDriver can_driver;
00212         TestNode node;
00213 
00214         NodeEnvironment(uavcan::NodeID node_id)
00215             : can_driver(clock)
00216             , node(can_driver, clock, node_id)
00217         { }
00218     };
00219 
00220     std::auto_ptr<NodeEnvironment> nodes[NumNodes];
00221 
00222     TestNetwork(uavcan::uint8_t first_node_id = 1)
00223     {
00224         for (uavcan::uint8_t i = 0; i < NumNodes; i++)
00225         {
00226             nodes[i].reset(new NodeEnvironment(uint8_t(first_node_id + i)));
00227         }
00228 
00229         for (uavcan::uint8_t i = 0; i < NumNodes; i++)
00230         {
00231             for (uavcan::uint8_t k = 0; k < NumNodes; k++)
00232             {
00233                 nodes[i]->can_driver.linkTogether(&nodes[k]->can_driver);
00234             }
00235         }
00236 
00237         for (uavcan::uint8_t i = 0; i < NumNodes; i++)
00238         {
00239             assert(nodes[i]->can_driver.others.size() == (NumNodes - 1));
00240         }
00241     }
00242 
00243     int spinAll(uavcan::MonotonicDuration duration)
00244     {
00245         assert(!duration.isNegative());
00246         unsigned nspins = unsigned(duration.toMSec() / NumNodes);
00247         nspins = nspins ? nspins : 1;
00248         while (nspins --> 0)
00249         {
00250             for (uavcan::uint8_t i = 0; i < NumNodes; i++)
00251             {
00252                 int ret = nodes[i]->node.spin(uavcan::MonotonicDuration::fromMSec(1));
00253                 if (ret < 0)
00254                 {
00255                     return ret;
00256                 }
00257             }
00258         }
00259         return 0;
00260     }
00261 
00262     TestNode& operator[](unsigned index)
00263     {
00264         if (index >= NumNodes)
00265         {
00266             throw std::out_of_range("No such test node");
00267         }
00268         return nodes[index]->node;
00269     }
00270 };