libvisiontransfer  7.1.0
imagetransfer.cpp
1 /*******************************************************************************
2  * Copyright (c) 2019 Nerian Vision GmbH
3  *
4  * Permission is hereby granted, free of charge, to any person obtaining a copy
5  * of this software and associated documentation files (the "Software"), to deal
6  * in the Software without restriction, including without limitation the rights
7  * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
8  * copies of the Software, and to permit persons to whom the Software is
9  * furnished to do so, subject to the following conditions:
10  *
11  * The above copyright notice and this permission notice shall be included in
12  * all copies or substantial portions of the Software.
13  *******************************************************************************/
14 
15 #include <cstdio>
16 #include <iostream>
17 #include <cstring>
18 #include <memory>
19 #include <string>
20 #include <vector>
21 #include <mutex>
22 #include "visiontransfer/imagetransfer.h"
23 #include "visiontransfer/exceptions.h"
24 #include "visiontransfer/datablockprotocol.h"
25 #include "visiontransfer/networking.h"
26 
27 using namespace std;
28 using namespace visiontransfer;
29 using namespace visiontransfer::internal;
30 
31 namespace visiontransfer {
32 
33 /*************** Pimpl class containing all private members ***********/
34 
35 class ImageTransfer::Pimpl {
36 public:
37  Pimpl(const char* address, const char* service, ImageProtocol::ProtocolType protType,
38  bool server, int bufferSize, int maxUdpPacketSize);
39  ~Pimpl();
40 
41  // Redeclaration of public members
42  void setRawTransferData(const ImagePair& metaData, unsigned char* rawData,
43  int firstTileWidth = 0, int secondTileWidth = 0, int validBytes = 0x7FFFFFFF);
44  void setRawValidBytes(int validBytes);
45  void setTransferImagePair(const ImagePair& imagePair);
46  TransferStatus transferData();
47  bool receiveImagePair(ImagePair& imagePair);
48  bool receivePartialImagePair(ImagePair& imagePair, int& validRows, bool& complete);
49  int getNumDroppedFrames() const;
50  bool isConnected() const;
51  void disconnect();
52  std::string getRemoteAddress() const;
53  bool tryAccept();
54 
55 private:
56  // Configuration parameters
58  bool isServer;
59  int bufferSize;
60  int maxUdpPacketSize;
61 
62  // Thread synchronization
63  std::recursive_mutex receiveMutex;
64  std::recursive_mutex sendMutex;
65 
66  // Transfer related members
67  SOCKET clientSocket;
68  SOCKET tcpServerSocket;
69  sockaddr_in remoteAddress;
70 
71  // Object for encoding and decoding the network protocol
72  std::unique_ptr<ImageProtocol> protocol;
73 
74  // Outstanding network message that still has to be transferred
75  int currentMsgLen;
76  int currentMsgOffset;
77  const unsigned char* currentMsg;
78 
79  // Socket configuration
80  void setSocketOptions();
81 
82  // Network socket initialization
83  void initTcpServer(const addrinfo* addressInfo);
84  void initTcpClient(const addrinfo* addressInfo);
85  void initUdp(const addrinfo* addressInfo);
86 
87  // Data reception
88  bool receiveNetworkData(bool block);
89 
90  // Data transmission
91  bool sendNetworkMessage(const unsigned char* msg, int length);
92  void sendPendingControlMessages();
93 
94  bool selectSocket(bool read, bool wait);
95 };
96 
97 /******************** Stubs for all public members ********************/
98 
99 ImageTransfer::ImageTransfer(const char* address, const char* service,
100  ImageProtocol::ProtocolType protType, bool server, int bufferSize, int maxUdpPacketSize):
101  pimpl(new Pimpl(address, service, protType, server, bufferSize, maxUdpPacketSize)) {
102  // All initialization in the pimpl class
103 }
104 
105 ImageTransfer::ImageTransfer(const DeviceInfo& device, int bufferSize, int maxUdpPacketSize):
106  pimpl(new Pimpl(device.getIpAddress().c_str(), "7681", static_cast<ImageProtocol::ProtocolType>(device.getNetworkProtocol()),
107  false, bufferSize, maxUdpPacketSize)) {
108  // All initialization in the pimpl class
109 }
110 
111 ImageTransfer::~ImageTransfer() {
112  delete pimpl;
113 }
114 
115 void ImageTransfer::setRawTransferData(const ImagePair& metaData, unsigned char* rawData,
116  int firstTileWidth, int secondTileWidth, int validBytes) {
117  pimpl->setRawTransferData(metaData, rawData, firstTileWidth, secondTileWidth, validBytes);
118 }
119 
120 void ImageTransfer::setRawValidBytes(int validBytes) {
121  pimpl->setRawValidBytes(validBytes);
122 }
123 
125  pimpl->setTransferImagePair(imagePair);
126 }
127 
129  return pimpl->transferData();
130 }
131 
133  return pimpl->receiveImagePair(imagePair);
134 }
135 
136 bool ImageTransfer::receivePartialImagePair(ImagePair& imagePair, int& validRows, bool& complete) {
137  return pimpl->receivePartialImagePair(imagePair, validRows, complete);
138 }
139 
141  return pimpl->getNumDroppedFrames();
142 }
143 
145  return pimpl->isConnected();
146 }
147 
149  pimpl->disconnect();
150 }
151 
152 std::string ImageTransfer::getRemoteAddress() const {
153  return pimpl->getRemoteAddress();
154 }
155 
157  return pimpl->tryAccept();
158 }
159 
160 /******************** Implementation in pimpl class *******************/
161 ImageTransfer::Pimpl::Pimpl(const char* address, const char* service,
162  ImageProtocol::ProtocolType protType, bool server, int
163  bufferSize, int maxUdpPacketSize)
164  : protType(protType), isServer(server), bufferSize(bufferSize),
165  maxUdpPacketSize(maxUdpPacketSize),
166  clientSocket(INVALID_SOCKET), tcpServerSocket(INVALID_SOCKET),
167  currentMsgLen(0), currentMsgOffset(0), currentMsg(nullptr) {
168 
169  Networking::initNetworking();
170 #ifndef _WIN32
171  // We don't want to be interrupted by the pipe signal
172  signal(SIGPIPE, SIG_IGN);
173 #endif
174 
175  memset(&remoteAddress, 0, sizeof(remoteAddress));
176 
177  // If address is null we use the any address
178  if(address == nullptr || string(address) == "") {
179  address = "0.0.0.0";
180  }
181 
182  addrinfo* addressInfo = Networking::resolveAddress(address, service);
183 
184  try {
185  if(protType == ImageProtocol::PROTOCOL_UDP) {
186  initUdp(addressInfo);
187  } else if(protType == ImageProtocol::PROTOCOL_TCP && isServer) {
188  initTcpServer(addressInfo);
189  } else {
190  initTcpClient(addressInfo);
191  }
192  } catch(...) {
193  freeaddrinfo(addressInfo);
194  throw;
195  }
196 
197  if(addressInfo != nullptr) {
198  freeaddrinfo(addressInfo);
199  }
200 }
201 
202 ImageTransfer::Pimpl::~Pimpl() {
203  if(clientSocket != INVALID_SOCKET) {
204  Networking::closeSocket(clientSocket);
205  }
206  if(tcpServerSocket != INVALID_SOCKET) {
207  Networking::closeSocket(tcpServerSocket);
208  }
209 }
210 
211 void ImageTransfer::Pimpl::initTcpClient(const addrinfo* addressInfo) {
212  protocol.reset(new ImageProtocol(isServer, ImageProtocol::PROTOCOL_TCP));
213  clientSocket = Networking::connectTcpSocket(addressInfo);
214  memcpy(&remoteAddress, addressInfo->ai_addr, sizeof(remoteAddress));
215 
216  // Set special socket options
217  setSocketOptions();
218 }
219 
220 void ImageTransfer::Pimpl::initTcpServer(const addrinfo* addressInfo) {
221  protocol.reset(new ImageProtocol(isServer, ImageProtocol::PROTOCOL_TCP));
222 
223  // Create socket
224  tcpServerSocket = ::socket(addressInfo->ai_family, addressInfo->ai_socktype,
225  addressInfo->ai_protocol);
226  if (tcpServerSocket == INVALID_SOCKET) {
227  TransferException ex("Error opening socket: " + string(strerror(errno)));
228  throw ex;
229  }
230 
231  // Enable reuse address
232  Networking::enableReuseAddress(tcpServerSocket, true);
233 
234  // Open a server port
235  Networking::bindSocket(tcpServerSocket, addressInfo);
236  clientSocket = INVALID_SOCKET;
237 
238  // Make the server socket non-blocking
239  Networking::setSocketBlocking(tcpServerSocket, false);
240 
241  // Listen on port
242  listen(tcpServerSocket, 1);
243 }
244 
245 void ImageTransfer::Pimpl::initUdp(const addrinfo* addressInfo) {
246  protocol.reset(new ImageProtocol(isServer, ImageProtocol::PROTOCOL_UDP, maxUdpPacketSize));
247  // Create sockets
248  clientSocket = socket(AF_INET, SOCK_DGRAM, 0);
249  if(clientSocket == INVALID_SOCKET) {
250  TransferException ex("Error creating receive socket: " + string(strerror(errno)));
251  throw ex;
252  }
253 
254  // Enable reuse address
255  Networking::enableReuseAddress(clientSocket, true);
256 
257  // Bind socket to port
258  if(isServer && addressInfo != nullptr) {
259  Networking::bindSocket(clientSocket, addressInfo);
260  }
261 
262  if(!isServer) {
263  memcpy(&remoteAddress, addressInfo->ai_addr, sizeof(remoteAddress));
264  }
265 
266  // Set special socket options
267  setSocketOptions();
268 }
269 
270 bool ImageTransfer::Pimpl::tryAccept() {
271  if(protType != ImageProtocol::PROTOCOL_TCP || ! isServer) {
272  throw TransferException("Connections can only be accepted in tcp server mode");
273  }
274 
275  unique_lock<recursive_mutex> recvLock(receiveMutex);
276  unique_lock<recursive_mutex> sendLock(sendMutex);
277 
278  // Accept one connection
279  SOCKET newSocket = Networking::acceptConnection(tcpServerSocket, remoteAddress);
280  if(newSocket == INVALID_SOCKET) {
281  // No connection
282  return false;
283  }
284 
285  if(clientSocket != INVALID_SOCKET) {
286  Networking::closeSocket(clientSocket);
287  }
288  clientSocket = newSocket;
289 
290  // Set special socket options
291  setSocketOptions();
292 
293  // Reset connection data
294  protocol->resetTransfer();
295  protocol->resetReception();
296  currentMsg = nullptr;
297 
298  return true;
299 }
300 
301 std::string ImageTransfer::Pimpl::getRemoteAddress() const {
302  unique_lock<recursive_mutex> lock(const_cast<recursive_mutex&>(sendMutex)); // either mutex will work
303 
304  if(remoteAddress.sin_family != AF_INET) {
305  return "";
306  }
307 
308  char strPort[11];
309  snprintf(strPort, sizeof(strPort), ":%d", remoteAddress.sin_port);
310 
311  return string(inet_ntoa(remoteAddress.sin_addr)) + strPort;
312 }
313 
314 void ImageTransfer::Pimpl::setSocketOptions() {
315  // Set the socket buffer sizes
316  if(bufferSize > 0) {
317  setsockopt(clientSocket, SOL_SOCKET, SO_RCVBUF, reinterpret_cast<char*>(&bufferSize), sizeof(bufferSize));
318  setsockopt(clientSocket, SOL_SOCKET, SO_SNDBUF, reinterpret_cast<char*>(&bufferSize), sizeof(bufferSize));
319  }
320 
321  Networking::setSocketTimeout(clientSocket, 500);
322  Networking::setSocketBlocking(clientSocket, true);
323 }
324 
325 void ImageTransfer::Pimpl::setRawTransferData(const ImagePair& metaData,
326  unsigned char* rawData, int firstTileWidth, int secondTileWidth, int validBytes) {
327  unique_lock<recursive_mutex> sendLock(sendMutex);
328  protocol->setRawTransferData(metaData, rawData, firstTileWidth, secondTileWidth, validBytes);
329  currentMsg = nullptr;
330 }
331 
332 void ImageTransfer::Pimpl::setRawValidBytes(int validBytes) {
333  unique_lock<recursive_mutex> sendLock(sendMutex);
334  protocol->setRawValidBytes(validBytes);
335 }
336 
337 void ImageTransfer::Pimpl::setTransferImagePair(const ImagePair& imagePair) {
338  unique_lock<recursive_mutex> sendLock(sendMutex);
339  protocol->setTransferImagePair(imagePair);
340  currentMsg = nullptr;
341 }
342 
343 ImageTransfer::TransferStatus ImageTransfer::Pimpl::transferData() {
344  unique_lock<recursive_mutex> lock(sendMutex);
345 
346  // First receive data in case a control message arrives
347  if(protType == ImageProtocol::PROTOCOL_UDP) {
348  receiveNetworkData(false);
349  }
350 
351  if(remoteAddress.sin_family != AF_INET || !protocol->isConnected()) {
352  return NOT_CONNECTED;
353  }
354 
355  // Get first message to transfer
356  if(currentMsg == nullptr) {
357  currentMsgOffset = 0;
358  currentMsg = protocol->getTransferMessage(currentMsgLen);
359 
360  if(currentMsg == nullptr) {
361  if(protocol->transferComplete()) {
362  return ALL_TRANSFERRED;
363  } else {
364  return NO_VALID_DATA;
365  }
366  }
367  }
368 
369  // Try transferring messages
370  bool dataTransferred = (currentMsg != nullptr);
371  while(currentMsg != nullptr) {
372  int writing = (int)(currentMsgLen - currentMsgOffset);
373 
374  if(sendNetworkMessage(&currentMsg[currentMsgOffset], writing)) {
375  // Get next message
376  currentMsgOffset = 0;
377  currentMsg = protocol->getTransferMessage(currentMsgLen);
378  } else {
379  return WOULD_BLOCK;
380  }
381  }
382 
383  if(dataTransferred && protType == ImageProtocol::PROTOCOL_TCP && protocol->transferComplete()) {
384  // Force a flush for TCP by turning the nagle algorithm off and on
385  int flag = 1;
386  setsockopt(clientSocket, IPPROTO_TCP, TCP_NODELAY, (char *) &flag, sizeof(int));
387  flag = 0;
388  setsockopt(clientSocket, IPPROTO_TCP, TCP_NODELAY, (char *) &flag, sizeof(int));
389  }
390 
391  // Also check for control messages at the end
392  if(protType == ImageProtocol::PROTOCOL_UDP) {
393  receiveNetworkData(false);
394  }
395 
396  if(protocol->transferComplete()) {
397  return ALL_TRANSFERRED;
398  } else {
399  return PARTIAL_TRANSFER;
400  }
401 }
402 
403 bool ImageTransfer::Pimpl::receiveImagePair(ImagePair& imagePair) {
404  int validRows = 0;
405  bool complete = false;
406 
407  std::chrono::steady_clock::time_point startTime = std::chrono::steady_clock::now();
408  while(!complete) {
409  if(!receivePartialImagePair(imagePair, validRows, complete)) {
410  return false;
411  }
412 
413  unsigned int time = static_cast<unsigned int>(std::chrono::duration_cast<std::chrono::milliseconds>(
414  std::chrono::steady_clock::now() - startTime).count());
415  if(time > 1000) {
416  return false;
417  }
418  }
419 
420  return true;
421 }
422 
423 bool ImageTransfer::Pimpl::receivePartialImagePair(ImagePair& imagePair,
424  int& validRows, bool& complete) {
425  unique_lock<recursive_mutex> lock(receiveMutex);
426 
427  // Try to receive further image data if needed
428  bool block = true;
429  while(!protocol->imagesReceived() && receiveNetworkData(block)) {
430  block = false;
431  }
432 
433  // Get received image
434  return protocol->getPartiallyReceivedImagePair(imagePair, validRows, complete);
435 }
436 
437 bool ImageTransfer::Pimpl::receiveNetworkData(bool block) {
438  unique_lock<recursive_mutex> lock = block ?
439  unique_lock<recursive_mutex>(receiveMutex) : unique_lock<recursive_mutex>(receiveMutex, std::try_to_lock);
440 
441  if(clientSocket == INVALID_SOCKET) {
442  return false; // Not connected
443  }
444 
445  // First send control messages if necessary
446  sendPendingControlMessages();
447 
448  if(!lock.owns_lock()) {
449  // Waiting for the lock would block this call
450  return false;
451  }
452 
453  // Test if the socket has data available
454  if(!block && !selectSocket(true, false)) {
455  return 0;
456  }
457 
458  int maxLength = 0;
459  char* buffer = reinterpret_cast<char*>(protocol->getNextReceiveBuffer(maxLength));
460 
461  // Receive data
462  sockaddr_in fromAddress;
463  socklen_t fromSize = sizeof(fromAddress);
464 
465  int bytesReceived = recvfrom(clientSocket, buffer, maxLength,
466  0, reinterpret_cast<sockaddr*>(&fromAddress), &fromSize);
467 
468  if(bytesReceived == 0 || (protType == ImageProtocol::PROTOCOL_TCP && bytesReceived < 0 && errno == WSAECONNRESET)) {
469  // Connection closed
470  disconnect();
471  } else if(bytesReceived < 0 && errno != EWOULDBLOCK && errno != EINTR &&
472  errno != ETIMEDOUT && errno != WSA_IO_PENDING && errno != WSAECONNRESET) {
473  TransferException ex("Error reading from socket: " + string(strerror(errno)));
474  throw ex;
475  } else if(bytesReceived > 0) {
476  protocol->processReceivedMessage(bytesReceived);
477  if(protocol->newClientConnected()) {
478  // We have just established a new connection
479  memcpy(&remoteAddress, &fromAddress, sizeof(remoteAddress));
480  }
481  }
482 
483  return bytesReceived > 0;
484 }
485 
486 void ImageTransfer::Pimpl::disconnect() {
487  // We just need to forget the remote address in order to
488  // disconnect
489  unique_lock<recursive_mutex> recvLock(receiveMutex);
490  unique_lock<recursive_mutex> sendLock(sendMutex);
491 
492  if(clientSocket != INVALID_SOCKET && protType == ImageProtocol::PROTOCOL_TCP) {
493  Networking::closeSocket(clientSocket);
494  }
495  memset(&remoteAddress, 0, sizeof(remoteAddress));
496 }
497 
498 bool ImageTransfer::Pimpl::isConnected() const {
499  unique_lock<recursive_mutex> lock(const_cast<recursive_mutex&>(sendMutex)); //either mutex will work
500 
501  return remoteAddress.sin_family == AF_INET && protocol->isConnected();
502 }
503 
504 bool ImageTransfer::Pimpl::sendNetworkMessage(const unsigned char* msg, int length) {
505  int written = 0;
506  if(protType == ImageProtocol::PROTOCOL_UDP) {
507  sockaddr_in destAddr;
508  SOCKET destSocket;
509  {
510  unique_lock<recursive_mutex> lock(sendMutex);
511  destAddr = remoteAddress;
512  destSocket = clientSocket;
513  }
514 
515  if(destAddr.sin_family != AF_INET) {
516  return false; // Not connected
517  }
518 
519  written = sendto(destSocket, reinterpret_cast<const char*>(msg), length, 0,
520  reinterpret_cast<sockaddr*>(&destAddr), sizeof(destAddr));
521  } else {
522  SOCKET destSocket;
523  {
524  unique_lock<recursive_mutex> lock(sendMutex);
525  destSocket = clientSocket;
526  }
527  written = send(destSocket, reinterpret_cast<const char*>(msg), length, 0);
528  }
529 
530  unsigned long sendError = errno;
531 
532  if(written < 0) {
533  if(sendError == EAGAIN || sendError == EWOULDBLOCK || sendError == ETIMEDOUT) {
534  // The socket is not yet ready for a new transfer
535  return false;
536  } else if(sendError == EPIPE) {
537  // The connection has been closed
538  disconnect();
539  return false;
540  } else {
541  TransferException ex("Error sending network packet: " + string(strerror(sendError)));
542  throw ex;
543  }
544  } else if(written != length) {
545  if(protType == ImageProtocol::PROTOCOL_UDP) {
546  // The message has been transmitted partially
547  throw TransferException("Unable to transmit complete UDP message");
548  } else {
549  // For TCP we can transmit the remaining data later
550  currentMsgOffset += written;
551  return false;
552  }
553  } else {
554  return true;
555  }
556 }
557 
558 void ImageTransfer::Pimpl::sendPendingControlMessages() {
559  const unsigned char* controlMsgData = nullptr;
560  int controlMsgLen = 0;
561 
562  while(true) {
563  unique_lock<recursive_mutex> lock(sendMutex);
564  if(remoteAddress.sin_family != AF_INET) {
565  return;
566  }
567 
568  controlMsgData = protocol->getNextControlMessage(controlMsgLen);
569 
570  if(controlMsgData != nullptr) {
571  sendNetworkMessage(controlMsgData, controlMsgLen);
572  } else {
573  break;
574  }
575  }
576 }
577 
578 int ImageTransfer::Pimpl::getNumDroppedFrames() const {
579  return protocol->getNumDroppedFrames();
580 }
581 
582 bool ImageTransfer::Pimpl::selectSocket(bool read, bool wait) {
583  SOCKET sock;
584  {
585  unique_lock<recursive_mutex> lock(sendMutex); // Either mutex will do
586  sock = clientSocket;
587  }
588 #ifdef _WIN32
589  fd_set fds;
590  struct timeval tv;
591  FD_ZERO(&fds);
592  FD_SET(sock, &fds);
593  tv.tv_sec = 0;
594  if(wait) {
595  tv.tv_usec = 100000;
596  } else {
597  tv.tv_usec = 0;
598  }
599 
600  if(select(sock+1, (read ? &fds : nullptr), (!read ? &fds : nullptr), nullptr, &tv) <= 0) {
601  // The socket is currently not ready
602  return false;
603  }
604 #else
605  // use poll() on non-Windows platform (glibc select() limitations)
606  constexpr int timeoutMillisec = 100;
607  pollfd pfd;
608  pfd.fd = sock;
609  pfd.events = POLLIN;
610  if (poll(&pfd, 1, wait ? timeoutMillisec: 0) <= 0) {
611  // The socket is currently not ready
612  return false;
613  }
614 #endif
615  // select (or poll) reported an event
616  return true;
617 }
618 
619 } // namespace
620 
bool receivePartialImagePair(ImagePair &imagePair, int &validRows, bool &complete)
Returns the received image pair, even if it is not yet complete.
std::string getRemoteAddress() const
Returns the address of the remote host.
int getNumDroppedFrames() const
Returns the number of frames that have been dropped since connecting to the current remote host...
The operation would block and blocking as been disabled.
Definition: imagetransfer.h:51
ImageTransfer(const char *address, const char *service="7681", ImageProtocol::ProtocolType protType=ImageProtocol::PROTOCOL_UDP, bool server=false, int bufferSize=1048576, int maxUdpPacketSize=1472)
Creates a new transfer object by manually specifying the target address.
void setRawTransferData(const ImagePair &metaData, unsigned char *rawData, int firstTileWidth=0, int secondTileWidth=0, int validBytes=0x7FFFFFFF)
Sets the raw pixel data for a partial image transmission.
The connection-less UDP transport protocol.
Definition: imageprotocol.h:46
bool tryAccept()
Tries to accept a client connection.
void disconnect()
Terminates the current connection.
A lightweight protocol for transferring image pairs.
Definition: imageprotocol.h:38
void setRawValidBytes(int validBytes)
Updates the number of valid bytes in a partial raw transmission.
No network connection has been established.
Definition: imagetransfer.h:54
TransferStatus transferData()
Performs a partial (or full) image transmission.
ProtocolType
Supported network protocols.
Definition: imageprotocol.h:41
bool isConnected() const
Returns true if a remote connection is established.
Aggregates information about a discovered device.
Definition: deviceinfo.h:47
There is currently no more data that could be transmitted.
Definition: imagetransfer.h:48
void setTransferImagePair(const ImagePair &imagePair)
Sets a new image pair that shall be transmitted.
The image pair has been transferred completely.
Definition: imagetransfer.h:41
Exception class that is used for all transfer exceptions.
Definition: exceptions.h:33
The connection oriented TCP transport protocol.
Definition: imageprotocol.h:43
A set of two images, which are usually the left camera image and the disparity map.
Definition: imagepair.h:33
TransferStatus
The result of a partial image transfer.
Definition: imagetransfer.h:39
bool receiveImagePair(ImagePair &imagePair)
Waits for and receives a new image pair.
Nerian Vision Technologies