transfer_test_helpers.hpp 11 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320
  1. /*
  2. * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
  3. */
  4. #pragma once
  5. #include <algorithm>
  6. #include <queue>
  7. #include <vector>
  8. #include <gtest/gtest.h>
  9. #include <uavcan/transport/transfer_listener.hpp>
  10. /**
  11. * UAVCAN transfer representation used in various tests.
  12. */
  13. struct Transfer
  14. {
  15. uavcan::MonotonicTime ts_monotonic;
  16. uavcan::UtcTime ts_utc;
  17. uavcan::TransferPriority priority;
  18. uavcan::TransferType transfer_type;
  19. uavcan::TransferID transfer_id;
  20. uavcan::NodeID src_node_id;
  21. uavcan::NodeID dst_node_id;
  22. uavcan::DataTypeDescriptor data_type;
  23. std::string payload;
  24. Transfer(const uavcan::IncomingTransfer& tr, const uavcan::DataTypeDescriptor& data_type)
  25. : ts_monotonic(tr.getMonotonicTimestamp())
  26. , ts_utc(tr.getUtcTimestamp())
  27. , priority(tr.getPriority())
  28. , transfer_type(tr.getTransferType())
  29. , transfer_id(tr.getTransferID())
  30. , src_node_id(tr.getSrcNodeID())
  31. , dst_node_id() // default is invalid
  32. , data_type(data_type)
  33. {
  34. unsigned offset = 0;
  35. while (true)
  36. {
  37. uint8_t buf[256];
  38. int res = tr.read(offset, buf, sizeof(buf));
  39. if (res < 0)
  40. {
  41. std::cout << "IncomingTransferContainer: read failure " << res << std::endl;
  42. exit(1);
  43. }
  44. if (res == 0)
  45. {
  46. break;
  47. }
  48. payload += std::string(reinterpret_cast<const char*>(buf), unsigned(res));
  49. offset += unsigned(res);
  50. }
  51. }
  52. Transfer(uavcan::MonotonicTime ts_monotonic, uavcan::UtcTime ts_utc, uavcan::TransferPriority priority,
  53. uavcan::TransferType transfer_type, uavcan::TransferID transfer_id, uavcan::NodeID src_node_id,
  54. uavcan::NodeID dst_node_id, const std::string& payload, const uavcan::DataTypeDescriptor& data_type)
  55. : ts_monotonic(ts_monotonic)
  56. , ts_utc(ts_utc)
  57. , priority(priority)
  58. , transfer_type(transfer_type)
  59. , transfer_id(transfer_id)
  60. , src_node_id(src_node_id)
  61. , dst_node_id(dst_node_id)
  62. , data_type(data_type)
  63. , payload(payload)
  64. { }
  65. Transfer(uint64_t ts_monotonic, uint64_t ts_utc, uavcan::TransferPriority priority,
  66. uavcan::TransferType transfer_type, uavcan::TransferID transfer_id, uavcan::NodeID src_node_id,
  67. uavcan::NodeID dst_node_id, const std::string& payload, const uavcan::DataTypeDescriptor& data_type)
  68. : ts_monotonic(uavcan::MonotonicTime::fromUSec(ts_monotonic))
  69. , ts_utc(uavcan::UtcTime::fromUSec(ts_utc))
  70. , priority(priority)
  71. , transfer_type(transfer_type)
  72. , transfer_id(transfer_id)
  73. , src_node_id(src_node_id)
  74. , dst_node_id(dst_node_id)
  75. , data_type(data_type)
  76. , payload(payload)
  77. { }
  78. bool operator==(const Transfer& rhs) const
  79. {
  80. return
  81. (ts_monotonic == rhs.ts_monotonic) &&
  82. ((!ts_utc.isZero() && !rhs.ts_utc.isZero()) ? (ts_utc == rhs.ts_utc) : true) &&
  83. (priority == rhs.priority) &&
  84. (transfer_type == rhs.transfer_type) &&
  85. (transfer_id == rhs.transfer_id) &&
  86. (src_node_id == rhs.src_node_id) &&
  87. ((dst_node_id.isValid() && rhs.dst_node_id.isValid()) ? (dst_node_id == rhs.dst_node_id) : true) &&
  88. (data_type == rhs.data_type) &&
  89. (payload == rhs.payload);
  90. }
  91. std::string toString() const
  92. {
  93. std::ostringstream os;
  94. os << "ts_m=" << ts_monotonic
  95. << " ts_utc=" << ts_utc
  96. << " prio=" << int(priority.get())
  97. << " tt=" << int(transfer_type)
  98. << " tid=" << int(transfer_id.get())
  99. << " snid=" << int(src_node_id.get())
  100. << " dnid=" << int(dst_node_id.get())
  101. << " dtid=" << int(data_type.getID().get())
  102. << "\n\t'" << payload << "'";
  103. return os.str();
  104. }
  105. };
  106. /**
  107. * This subscriber accepts any types of transfers - this makes testing easier.
  108. * In reality, uavcan::TransferListener should accept only specific transfer types
  109. * which are dispatched/filtered by uavcan::Dispatcher.
  110. */
  111. class TestListener : public uavcan::TransferListener
  112. {
  113. typedef uavcan::TransferListener Base;
  114. std::queue<Transfer> transfers_;
  115. public:
  116. TestListener(uavcan::TransferPerfCounter& perf, const uavcan::DataTypeDescriptor& data_type,
  117. uavcan::uint16_t max_buffer_size, uavcan::IPoolAllocator& allocator)
  118. : Base(perf, data_type, max_buffer_size, allocator)
  119. { }
  120. void handleIncomingTransfer(uavcan::IncomingTransfer& transfer)
  121. {
  122. const Transfer rx(transfer, Base::getDataTypeDescriptor());
  123. transfers_.push(rx);
  124. std::cout << "Received transfer: " << rx.toString() << std::endl;
  125. const bool single_frame = dynamic_cast<uavcan::SingleFrameIncomingTransfer*>(&transfer) != UAVCAN_NULLPTR;
  126. const bool anonymous = single_frame &&
  127. transfer.getSrcNodeID().isBroadcast() &&
  128. (transfer.getTransferType() == uavcan::TransferTypeMessageBroadcast);
  129. ASSERT_EQ(anonymous, transfer.isAnonymousTransfer());
  130. }
  131. bool matchAndPop(const Transfer& reference)
  132. {
  133. if (transfers_.empty())
  134. {
  135. std::cout << "No received transfers" << std::endl;
  136. return false;
  137. }
  138. const Transfer tr = transfers_.front();
  139. transfers_.pop();
  140. const bool res = (tr == reference);
  141. if (!res)
  142. {
  143. std::cout << "TestSubscriber: Transfer mismatch:\n"
  144. << "Expected: " << reference.toString() << "\n"
  145. << "Received: " << tr.toString() << std::endl;
  146. }
  147. return res;
  148. }
  149. unsigned getNumReceivedTransfers() const { return static_cast<unsigned>(transfers_.size()); }
  150. bool isEmpty() const { return transfers_.empty(); }
  151. };
  152. namespace
  153. {
  154. std::vector<uavcan::RxFrame> serializeTransfer(const Transfer& transfer)
  155. {
  156. const bool need_crc = transfer.payload.length() > (sizeof(uavcan::CanFrame::data) - 1);
  157. std::vector<uint8_t> raw_payload;
  158. if (need_crc)
  159. {
  160. uavcan::TransferCRC payload_crc = transfer.data_type.getSignature().toTransferCRC();
  161. payload_crc.add(reinterpret_cast<const uint8_t*>(transfer.payload.c_str()), uint16_t(transfer.payload.length()));
  162. // Little endian
  163. raw_payload.push_back(uint8_t(payload_crc.get() & 0xFF));
  164. raw_payload.push_back(uint8_t((payload_crc.get() >> 8) & 0xFF));
  165. }
  166. raw_payload.insert(raw_payload.end(), transfer.payload.begin(), transfer.payload.end());
  167. std::vector<uavcan::RxFrame> output;
  168. unsigned offset = 0;
  169. uavcan::MonotonicTime ts_monotonic = transfer.ts_monotonic;
  170. uavcan::UtcTime ts_utc = transfer.ts_utc;
  171. uavcan::Frame frm(transfer.data_type.getID(), transfer.transfer_type, transfer.src_node_id,
  172. transfer.dst_node_id, transfer.transfer_id);
  173. frm.setStartOfTransfer(true);
  174. frm.setPriority(transfer.priority);
  175. while (true)
  176. {
  177. const int bytes_left = int(raw_payload.size()) - int(offset);
  178. EXPECT_TRUE(bytes_left >= 0);
  179. const int spres = frm.setPayload(&*(raw_payload.begin() + offset), unsigned(bytes_left));
  180. if (spres < 0)
  181. {
  182. std::cerr << ">_<" << std::endl;
  183. std::exit(1);
  184. }
  185. if (spres == bytes_left)
  186. {
  187. frm.setEndOfTransfer(true);
  188. }
  189. offset += unsigned(spres);
  190. const uavcan::RxFrame rxfrm(frm, ts_monotonic, ts_utc, 0);
  191. ts_monotonic += uavcan::MonotonicDuration::fromUSec(1);
  192. ts_utc += uavcan::UtcDuration::fromUSec(1);
  193. output.push_back(rxfrm);
  194. if (frm.isEndOfTransfer())
  195. {
  196. break;
  197. }
  198. frm.setStartOfTransfer(false);
  199. frm.flipToggle();
  200. }
  201. return output;
  202. }
  203. inline uavcan::DataTypeDescriptor makeDataType(uavcan::DataTypeKind kind, uint16_t id, const char* name = "")
  204. {
  205. const uavcan::DataTypeSignature signature((uint64_t(kind) << 16) | uint16_t(id << 8) | uint16_t(id & 0xFF));
  206. return uavcan::DataTypeDescriptor(kind, id, signature, name);
  207. }
  208. }
  209. class IncomingTransferEmulatorBase
  210. {
  211. uavcan::MonotonicTime ts_;
  212. uavcan::TransferID tid_;
  213. uavcan::NodeID dst_node_id_;
  214. public:
  215. IncomingTransferEmulatorBase(uavcan::NodeID dst_node_id)
  216. : dst_node_id_(dst_node_id)
  217. { }
  218. virtual ~IncomingTransferEmulatorBase() { }
  219. Transfer makeTransfer(uavcan::TransferPriority priority, uavcan::TransferType transfer_type,
  220. uint8_t source_node_id, const std::string& payload, const uavcan::DataTypeDescriptor& type,
  221. uavcan::NodeID dst_node_id_override = uavcan::NodeID())
  222. {
  223. ts_ += uavcan::MonotonicDuration::fromUSec(100);
  224. const uavcan::UtcTime utc = uavcan::UtcTime::fromUSec(ts_.toUSec() + 1000000000ul);
  225. const uavcan::NodeID dst_node_id = (transfer_type == uavcan::TransferTypeMessageBroadcast) ?
  226. uavcan::NodeID::Broadcast :
  227. (dst_node_id_override.isValid() ? dst_node_id_override : dst_node_id_);
  228. const Transfer tr(ts_, utc, priority, transfer_type, tid_, source_node_id, dst_node_id, payload, type);
  229. tid_.increment();
  230. return tr;
  231. }
  232. virtual void sendOneFrame(const uavcan::RxFrame& frame) = 0;
  233. void send(const std::vector<std::vector<uavcan::RxFrame> >& sers)
  234. {
  235. unsigned index = 0;
  236. while (true)
  237. {
  238. // Sending all transfers concurrently
  239. bool all_empty = true;
  240. for (std::vector<std::vector<uavcan::RxFrame> >::const_iterator it = sers.begin(); it != sers.end(); ++it)
  241. {
  242. if (it->size() <= index)
  243. {
  244. continue;
  245. }
  246. all_empty = false;
  247. std::cout << "Incoming Transfer Emulator: Sending: " << it->at(index).toString() << std::endl;
  248. sendOneFrame(it->at(index));
  249. }
  250. index++;
  251. if (all_empty)
  252. {
  253. break;
  254. }
  255. }
  256. }
  257. void send(const Transfer* transfers, unsigned num_transfers)
  258. {
  259. std::vector<std::vector<uavcan::RxFrame> > sers;
  260. while (num_transfers--)
  261. {
  262. sers.push_back(serializeTransfer(*transfers++));
  263. }
  264. send(sers);
  265. }
  266. template <int SIZE> void send(const Transfer (&transfers)[SIZE]) { send(transfers, SIZE); }
  267. };
  268. /**
  269. * Zero allocator - always fails
  270. */
  271. class NullAllocator : public uavcan::IPoolAllocator
  272. {
  273. public:
  274. virtual void* allocate(std::size_t) { return UAVCAN_NULLPTR; }
  275. virtual void deallocate(const void*) { }
  276. virtual uint16_t getBlockCapacity() const { return 0; }
  277. };