transfer_test_helpers.hpp

00001 /*
00002  * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
00003  */
00004 
00005 #pragma once
00006 
00007 #include <algorithm>
00008 #include <queue>
00009 #include <vector>
00010 #include <gtest/gtest.h>
00011 #include <uavcan/transport/transfer_listener.hpp>
00012 
00013 /**
00014  * UAVCAN transfer representation used in various tests.
00015  */
00016 struct Transfer
00017 {
00018     uavcan::MonotonicTime ts_monotonic;
00019     uavcan::UtcTime ts_utc;
00020     uavcan::TransferPriority priority;
00021     uavcan::TransferType transfer_type;
00022     uavcan::TransferID transfer_id;
00023     uavcan::NodeID src_node_id;
00024     uavcan::NodeID dst_node_id;
00025     uavcan::DataTypeDescriptor data_type;
00026     std::string payload;
00027 
00028     Transfer(const uavcan::IncomingTransfer& tr, const uavcan::DataTypeDescriptor& data_type)
00029         : ts_monotonic(tr.getMonotonicTimestamp())
00030         , ts_utc(tr.getUtcTimestamp())
00031         , priority(tr.getPriority())
00032         , transfer_type(tr.getTransferType())
00033         , transfer_id(tr.getTransferID())
00034         , src_node_id(tr.getSrcNodeID())
00035         , dst_node_id() // default is invalid
00036         , data_type(data_type)
00037     {
00038         unsigned offset = 0;
00039         while (true)
00040         {
00041             uint8_t buf[256];
00042             int res = tr.read(offset, buf, sizeof(buf));
00043             if (res < 0)
00044             {
00045                 std::cout << "IncomingTransferContainer: read failure " << res << std::endl;
00046                 exit(1);
00047             }
00048             if (res == 0)
00049             {
00050                 break;
00051             }
00052             payload += std::string(reinterpret_cast<const char*>(buf), unsigned(res));
00053             offset += unsigned(res);
00054         }
00055     }
00056 
00057     Transfer(uavcan::MonotonicTime ts_monotonic, uavcan::UtcTime ts_utc, uavcan::TransferPriority priority,
00058              uavcan::TransferType transfer_type, uavcan::TransferID transfer_id, uavcan::NodeID src_node_id,
00059              uavcan::NodeID dst_node_id, const std::string& payload, const uavcan::DataTypeDescriptor& data_type)
00060         : ts_monotonic(ts_monotonic)
00061         , ts_utc(ts_utc)
00062         , priority(priority)
00063         , transfer_type(transfer_type)
00064         , transfer_id(transfer_id)
00065         , src_node_id(src_node_id)
00066         , dst_node_id(dst_node_id)
00067         , data_type(data_type)
00068         , payload(payload)
00069     { }
00070 
00071     Transfer(uint64_t ts_monotonic, uint64_t ts_utc, uavcan::TransferPriority priority,
00072              uavcan::TransferType transfer_type, uavcan::TransferID transfer_id, uavcan::NodeID src_node_id,
00073              uavcan::NodeID dst_node_id, const std::string& payload, const uavcan::DataTypeDescriptor& data_type)
00074         : ts_monotonic(uavcan::MonotonicTime::fromUSec(ts_monotonic))
00075         , ts_utc(uavcan::UtcTime::fromUSec(ts_utc))
00076         , priority(priority)
00077         , transfer_type(transfer_type)
00078         , transfer_id(transfer_id)
00079         , src_node_id(src_node_id)
00080         , dst_node_id(dst_node_id)
00081         , data_type(data_type)
00082         , payload(payload)
00083     { }
00084 
00085     bool operator==(const Transfer& rhs) const
00086     {
00087         return
00088             (ts_monotonic  == rhs.ts_monotonic) &&
00089             ((!ts_utc.isZero() && !rhs.ts_utc.isZero()) ? (ts_utc == rhs.ts_utc) : true) &&
00090             (priority      == rhs.priority) &&
00091             (transfer_type == rhs.transfer_type) &&
00092             (transfer_id   == rhs.transfer_id) &&
00093             (src_node_id   == rhs.src_node_id) &&
00094             ((dst_node_id.isValid() && rhs.dst_node_id.isValid()) ? (dst_node_id == rhs.dst_node_id) : true) &&
00095             (data_type     == rhs.data_type) &&
00096             (payload       == rhs.payload);
00097     }
00098 
00099     std::string toString() const
00100     {
00101         std::ostringstream os;
00102         os << "ts_m="    << ts_monotonic
00103            << " ts_utc=" << ts_utc
00104            << " prio="   << int(priority.get())
00105            << " tt="     << int(transfer_type)
00106            << " tid="    << int(transfer_id.get())
00107            << " snid="   << int(src_node_id.get())
00108            << " dnid="   << int(dst_node_id.get())
00109            << " dtid="   << int(data_type.getID().get())
00110            << "\n\t'" << payload << "'";
00111         return os.str();
00112     }
00113 };
00114 
00115 /**
00116  * This subscriber accepts any types of transfers - this makes testing easier.
00117  * In reality, uavcan::TransferListener should accept only specific transfer types
00118  * which are dispatched/filtered by uavcan::Dispatcher.
00119  */
00120 class TestListener : public uavcan::TransferListener
00121 {
00122     typedef uavcan::TransferListener Base;
00123 
00124     std::queue<Transfer> transfers_;
00125 
00126 public:
00127     TestListener(uavcan::TransferPerfCounter& perf, const uavcan::DataTypeDescriptor& data_type,
00128                  uavcan::uint16_t max_buffer_size, uavcan::IPoolAllocator& allocator)
00129         : Base(perf, data_type, max_buffer_size, allocator)
00130     { }
00131 
00132     void handleIncomingTransfer(uavcan::IncomingTransfer& transfer)
00133     {
00134         const Transfer rx(transfer, Base::getDataTypeDescriptor());
00135         transfers_.push(rx);
00136         std::cout << "Received transfer: " << rx.toString() << std::endl;
00137 
00138         const bool single_frame = dynamic_cast<uavcan::SingleFrameIncomingTransfer*>(&transfer) != UAVCAN_NULLPTR;
00139 
00140         const bool anonymous = single_frame &&
00141                                transfer.getSrcNodeID().isBroadcast() &&
00142                                (transfer.getTransferType() == uavcan::TransferTypeMessageBroadcast);
00143 
00144         ASSERT_EQ(anonymous, transfer.isAnonymousTransfer());
00145     }
00146 
00147     bool matchAndPop(const Transfer& reference)
00148     {
00149         if (transfers_.empty())
00150         {
00151             std::cout << "No received transfers" << std::endl;
00152             return false;
00153         }
00154 
00155         const Transfer tr = transfers_.front();
00156         transfers_.pop();
00157 
00158         const bool res = (tr == reference);
00159         if (!res)
00160         {
00161             std::cout << "TestSubscriber: Transfer mismatch:\n"
00162                       << "Expected: " << reference.toString() << "\n"
00163                       << "Received: " << tr.toString() << std::endl;
00164         }
00165         return res;
00166     }
00167 
00168     unsigned getNumReceivedTransfers() const { return static_cast<unsigned>(transfers_.size()); }
00169     bool isEmpty() const { return transfers_.empty(); }
00170 };
00171 
00172 
00173 namespace
00174 {
00175 
00176 std::vector<uavcan::RxFrame> serializeTransfer(const Transfer& transfer)
00177 {
00178     const bool need_crc = transfer.payload.length() > (sizeof(uavcan::CanFrame::data) - 1);
00179 
00180     std::vector<uint8_t> raw_payload;
00181     if (need_crc)
00182     {
00183         uavcan::TransferCRC payload_crc = transfer.data_type.getSignature().toTransferCRC();
00184         payload_crc.add(reinterpret_cast<const uint8_t*>(transfer.payload.c_str()), uint16_t(transfer.payload.length()));
00185         // Little endian
00186         raw_payload.push_back(uint8_t(payload_crc.get() & 0xFF));
00187         raw_payload.push_back(uint8_t((payload_crc.get() >> 8) & 0xFF));
00188     }
00189     raw_payload.insert(raw_payload.end(), transfer.payload.begin(), transfer.payload.end());
00190 
00191     std::vector<uavcan::RxFrame> output;
00192     unsigned offset = 0;
00193     uavcan::MonotonicTime ts_monotonic = transfer.ts_monotonic;
00194     uavcan::UtcTime ts_utc = transfer.ts_utc;
00195 
00196     uavcan::Frame frm(transfer.data_type.getID(), transfer.transfer_type, transfer.src_node_id,
00197                       transfer.dst_node_id, transfer.transfer_id);
00198     frm.setStartOfTransfer(true);
00199     frm.setPriority(transfer.priority);
00200 
00201     while (true)
00202     {
00203         const int bytes_left = int(raw_payload.size()) - int(offset);
00204         EXPECT_TRUE(bytes_left >= 0);
00205 
00206         const int spres = frm.setPayload(&*(raw_payload.begin() + offset), unsigned(bytes_left));
00207         if (spres < 0)
00208         {
00209             std::cerr << ">_<" << std::endl;
00210             std::exit(1);
00211         }
00212         if (spres == bytes_left)
00213         {
00214             frm.setEndOfTransfer(true);
00215         }
00216 
00217         offset += unsigned(spres);
00218 
00219         const uavcan::RxFrame rxfrm(frm, ts_monotonic, ts_utc, 0);
00220         ts_monotonic += uavcan::MonotonicDuration::fromUSec(1);
00221         ts_utc += uavcan::UtcDuration::fromUSec(1);
00222 
00223         output.push_back(rxfrm);
00224         if (frm.isEndOfTransfer())
00225         {
00226             break;
00227         }
00228 
00229         frm.setStartOfTransfer(false);
00230         frm.flipToggle();
00231     }
00232     return output;
00233 }
00234 
00235 inline uavcan::DataTypeDescriptor makeDataType(uavcan::DataTypeKind kind, uint16_t id, const char* name = "")
00236 {
00237     const uavcan::DataTypeSignature signature((uint64_t(kind) << 16) | uint16_t(id << 8) | uint16_t(id & 0xFF));
00238     return uavcan::DataTypeDescriptor(kind, id, signature, name);
00239 }
00240 
00241 }
00242 
00243 
00244 class IncomingTransferEmulatorBase
00245 {
00246     uavcan::MonotonicTime ts_;
00247     uavcan::TransferID tid_;
00248     uavcan::NodeID dst_node_id_;
00249 
00250 public:
00251     IncomingTransferEmulatorBase(uavcan::NodeID dst_node_id)
00252         : dst_node_id_(dst_node_id)
00253     { }
00254 
00255     virtual ~IncomingTransferEmulatorBase() { }
00256 
00257     Transfer makeTransfer(uavcan::TransferPriority priority, uavcan::TransferType transfer_type,
00258                           uint8_t source_node_id, const std::string& payload, const uavcan::DataTypeDescriptor& type,
00259                           uavcan::NodeID dst_node_id_override = uavcan::NodeID())
00260     {
00261         ts_ += uavcan::MonotonicDuration::fromUSec(100);
00262         const uavcan::UtcTime utc = uavcan::UtcTime::fromUSec(ts_.toUSec() + 1000000000ul);
00263         const uavcan::NodeID dst_node_id = (transfer_type == uavcan::TransferTypeMessageBroadcast) ?
00264                                            uavcan::NodeID::Broadcast :
00265                                            (dst_node_id_override.isValid() ? dst_node_id_override : dst_node_id_);
00266         const Transfer tr(ts_, utc, priority, transfer_type, tid_, source_node_id, dst_node_id, payload, type);
00267         tid_.increment();
00268         return tr;
00269     }
00270 
00271     virtual void sendOneFrame(const uavcan::RxFrame& frame) = 0;
00272 
00273     void send(const std::vector<std::vector<uavcan::RxFrame> >& sers)
00274     {
00275         unsigned index = 0;
00276         while (true)
00277         {
00278             // Sending all transfers concurrently
00279             bool all_empty = true;
00280             for (std::vector<std::vector<uavcan::RxFrame> >::const_iterator it = sers.begin(); it != sers.end(); ++it)
00281             {
00282                 if (it->size() <= index)
00283                 {
00284                     continue;
00285                 }
00286                 all_empty = false;
00287                 std::cout << "Incoming Transfer Emulator: Sending: " << it->at(index).toString() << std::endl;
00288                 sendOneFrame(it->at(index));
00289             }
00290             index++;
00291             if (all_empty)
00292             {
00293                 break;
00294             }
00295         }
00296     }
00297 
00298     void send(const Transfer* transfers, unsigned num_transfers)
00299     {
00300         std::vector<std::vector<uavcan::RxFrame> > sers;
00301         while (num_transfers--)
00302         {
00303             sers.push_back(serializeTransfer(*transfers++));
00304         }
00305         send(sers);
00306     }
00307 
00308     template <int SIZE> void send(const Transfer (&transfers)[SIZE]) { send(transfers, SIZE); }
00309 };
00310 
00311 /**
00312  * Zero allocator - always fails
00313  */
00314 class NullAllocator : public uavcan::IPoolAllocator
00315 {
00316 public:
00317     virtual void* allocate(std::size_t) { return UAVCAN_NULLPTR; }
00318     virtual void deallocate(const void*) { }
00319     virtual uint16_t getBlockCapacity() const { return 0; }
00320 };