22 #include "visiontransfer/imagetransfer.h" 23 #include "visiontransfer/exceptions.h" 24 #include "visiontransfer/datablockprotocol.h" 25 #include "visiontransfer/networking.h" 35 class ImageTransfer::Pimpl {
38 bool server,
int bufferSize,
int maxUdpPacketSize);
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;
52 std::string getRemoteAddress()
const;
63 std::recursive_mutex receiveMutex;
64 std::recursive_mutex sendMutex;
68 SOCKET tcpServerSocket;
69 sockaddr_in remoteAddress;
72 std::unique_ptr<ImageProtocol> protocol;
77 const unsigned char* currentMsg;
80 void setSocketOptions();
83 void initTcpServer(
const addrinfo* addressInfo);
84 void initTcpClient(
const addrinfo* addressInfo);
85 void initUdp(
const addrinfo* addressInfo);
88 bool receiveNetworkData(
bool block);
91 bool sendNetworkMessage(
const unsigned char* msg,
int length);
92 void sendPendingControlMessages();
94 bool selectSocket(
bool read,
bool wait);
99 ImageTransfer::ImageTransfer(
const char* address,
const char* service,
101 pimpl(new Pimpl(address, service, protType, server, bufferSize, maxUdpPacketSize)) {
106 pimpl(new Pimpl(device.getIpAddress().c_str(),
"7681", static_cast<
ImageProtocol::ProtocolType>(device.getNetworkProtocol()),
107 false, bufferSize, maxUdpPacketSize)) {
111 ImageTransfer::~ImageTransfer() {
116 int firstTileWidth,
int secondTileWidth,
int validBytes) {
117 pimpl->setRawTransferData(metaData, rawData, firstTileWidth, secondTileWidth, validBytes);
121 pimpl->setRawValidBytes(validBytes);
125 pimpl->setTransferImagePair(imagePair);
129 return pimpl->transferData();
133 return pimpl->receiveImagePair(imagePair);
137 return pimpl->receivePartialImagePair(imagePair, validRows, complete);
141 return pimpl->getNumDroppedFrames();
145 return pimpl->isConnected();
153 return pimpl->getRemoteAddress();
157 return pimpl->tryAccept();
161 ImageTransfer::Pimpl::Pimpl(
const char* address,
const char* service,
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) {
169 Networking::initNetworking();
172 signal(SIGPIPE, SIG_IGN);
175 memset(&remoteAddress, 0,
sizeof(remoteAddress));
178 if(address ==
nullptr ||
string(address) ==
"") {
182 addrinfo* addressInfo = Networking::resolveAddress(address, service);
186 initUdp(addressInfo);
188 initTcpServer(addressInfo);
190 initTcpClient(addressInfo);
193 freeaddrinfo(addressInfo);
197 if(addressInfo !=
nullptr) {
198 freeaddrinfo(addressInfo);
202 ImageTransfer::Pimpl::~Pimpl() {
203 if(clientSocket != INVALID_SOCKET) {
204 Networking::closeSocket(clientSocket);
206 if(tcpServerSocket != INVALID_SOCKET) {
207 Networking::closeSocket(tcpServerSocket);
211 void ImageTransfer::Pimpl::initTcpClient(
const addrinfo* addressInfo) {
213 clientSocket = Networking::connectTcpSocket(addressInfo);
214 memcpy(&remoteAddress, addressInfo->ai_addr,
sizeof(remoteAddress));
220 void ImageTransfer::Pimpl::initTcpServer(
const addrinfo* addressInfo) {
224 tcpServerSocket = ::socket(addressInfo->ai_family, addressInfo->ai_socktype,
225 addressInfo->ai_protocol);
226 if (tcpServerSocket == INVALID_SOCKET) {
232 Networking::enableReuseAddress(tcpServerSocket,
true);
235 Networking::bindSocket(tcpServerSocket, addressInfo);
236 clientSocket = INVALID_SOCKET;
239 Networking::setSocketBlocking(tcpServerSocket,
false);
242 listen(tcpServerSocket, 1);
245 void ImageTransfer::Pimpl::initUdp(
const addrinfo* addressInfo) {
248 clientSocket = socket(AF_INET, SOCK_DGRAM, 0);
249 if(clientSocket == INVALID_SOCKET) {
250 TransferException ex(
"Error creating receive socket: " +
string(strerror(errno)));
255 Networking::enableReuseAddress(clientSocket,
true);
258 if(isServer && addressInfo !=
nullptr) {
259 Networking::bindSocket(clientSocket, addressInfo);
263 memcpy(&remoteAddress, addressInfo->ai_addr,
sizeof(remoteAddress));
270 bool ImageTransfer::Pimpl::tryAccept() {
275 unique_lock<recursive_mutex> recvLock(receiveMutex);
276 unique_lock<recursive_mutex> sendLock(sendMutex);
279 SOCKET newSocket = Networking::acceptConnection(tcpServerSocket, remoteAddress);
280 if(newSocket == INVALID_SOCKET) {
285 if(clientSocket != INVALID_SOCKET) {
286 Networking::closeSocket(clientSocket);
288 clientSocket = newSocket;
294 protocol->resetTransfer();
295 protocol->resetReception();
296 currentMsg =
nullptr;
301 std::string ImageTransfer::Pimpl::getRemoteAddress()
const {
302 unique_lock<recursive_mutex> lock(const_cast<recursive_mutex&>(sendMutex));
304 if(remoteAddress.sin_family != AF_INET) {
309 snprintf(strPort,
sizeof(strPort),
":%d", remoteAddress.sin_port);
311 return string(inet_ntoa(remoteAddress.sin_addr)) + strPort;
314 void ImageTransfer::Pimpl::setSocketOptions() {
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));
321 Networking::setSocketTimeout(clientSocket, 500);
322 Networking::setSocketBlocking(clientSocket,
true);
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;
332 void ImageTransfer::Pimpl::setRawValidBytes(
int validBytes) {
333 unique_lock<recursive_mutex> sendLock(sendMutex);
334 protocol->setRawValidBytes(validBytes);
337 void ImageTransfer::Pimpl::setTransferImagePair(
const ImagePair& imagePair) {
338 unique_lock<recursive_mutex> sendLock(sendMutex);
339 protocol->setTransferImagePair(imagePair);
340 currentMsg =
nullptr;
344 unique_lock<recursive_mutex> lock(sendMutex);
348 receiveNetworkData(
false);
351 if(remoteAddress.sin_family != AF_INET || !protocol->isConnected()) {
356 if(currentMsg ==
nullptr) {
357 currentMsgOffset = 0;
358 currentMsg = protocol->getTransferMessage(currentMsgLen);
360 if(currentMsg ==
nullptr) {
361 if(protocol->transferComplete()) {
370 bool dataTransferred = (currentMsg !=
nullptr);
371 while(currentMsg !=
nullptr) {
372 int writing = (int)(currentMsgLen - currentMsgOffset);
374 if(sendNetworkMessage(¤tMsg[currentMsgOffset], writing)) {
376 currentMsgOffset = 0;
377 currentMsg = protocol->getTransferMessage(currentMsgLen);
386 setsockopt(clientSocket, IPPROTO_TCP, TCP_NODELAY, (
char *) &flag,
sizeof(
int));
388 setsockopt(clientSocket, IPPROTO_TCP, TCP_NODELAY, (
char *) &flag,
sizeof(
int));
393 receiveNetworkData(
false);
396 if(protocol->transferComplete()) {
403 bool ImageTransfer::Pimpl::receiveImagePair(
ImagePair& imagePair) {
405 bool complete =
false;
407 std::chrono::steady_clock::time_point startTime = std::chrono::steady_clock::now();
413 unsigned int time =
static_cast<unsigned int>(std::chrono::duration_cast<std::chrono::milliseconds>(
414 std::chrono::steady_clock::now() - startTime).count());
423 bool ImageTransfer::Pimpl::receivePartialImagePair(
ImagePair& imagePair,
424 int& validRows,
bool& complete) {
425 unique_lock<recursive_mutex> lock(receiveMutex);
429 while(!protocol->imagesReceived() && receiveNetworkData(block)) {
434 return protocol->getPartiallyReceivedImagePair(imagePair, validRows, complete);
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);
441 if(clientSocket == INVALID_SOCKET) {
446 sendPendingControlMessages();
448 if(!lock.owns_lock()) {
454 if(!block && !selectSocket(
true,
false)) {
459 char* buffer =
reinterpret_cast<char*
>(protocol->getNextReceiveBuffer(maxLength));
462 sockaddr_in fromAddress;
463 socklen_t fromSize =
sizeof(fromAddress);
465 int bytesReceived = recvfrom(clientSocket, buffer, maxLength,
466 0, reinterpret_cast<sockaddr*>(&fromAddress), &fromSize);
471 }
else if(bytesReceived < 0 && errno != EWOULDBLOCK && errno != EINTR &&
472 errno != ETIMEDOUT && errno != WSA_IO_PENDING && errno != WSAECONNRESET) {
475 }
else if(bytesReceived > 0) {
476 protocol->processReceivedMessage(bytesReceived);
477 if(protocol->newClientConnected()) {
479 memcpy(&remoteAddress, &fromAddress,
sizeof(remoteAddress));
483 return bytesReceived > 0;
486 void ImageTransfer::Pimpl::disconnect() {
489 unique_lock<recursive_mutex> recvLock(receiveMutex);
490 unique_lock<recursive_mutex> sendLock(sendMutex);
493 Networking::closeSocket(clientSocket);
495 memset(&remoteAddress, 0,
sizeof(remoteAddress));
498 bool ImageTransfer::Pimpl::isConnected()
const {
499 unique_lock<recursive_mutex> lock(const_cast<recursive_mutex&>(sendMutex));
501 return remoteAddress.sin_family == AF_INET && protocol->isConnected();
504 bool ImageTransfer::Pimpl::sendNetworkMessage(
const unsigned char* msg,
int length) {
507 sockaddr_in destAddr;
510 unique_lock<recursive_mutex> lock(sendMutex);
511 destAddr = remoteAddress;
512 destSocket = clientSocket;
515 if(destAddr.sin_family != AF_INET) {
519 written = sendto(destSocket, reinterpret_cast<const char*>(msg), length, 0,
520 reinterpret_cast<sockaddr*>(&destAddr),
sizeof(destAddr));
524 unique_lock<recursive_mutex> lock(sendMutex);
525 destSocket = clientSocket;
527 written = send(destSocket, reinterpret_cast<const char*>(msg), length, 0);
530 unsigned long sendError = errno;
533 if(sendError == EAGAIN || sendError == EWOULDBLOCK || sendError == ETIMEDOUT) {
536 }
else if(sendError == EPIPE) {
541 TransferException ex(
"Error sending network packet: " +
string(strerror(sendError)));
544 }
else if(written != length) {
550 currentMsgOffset += written;
558 void ImageTransfer::Pimpl::sendPendingControlMessages() {
559 const unsigned char* controlMsgData =
nullptr;
560 int controlMsgLen = 0;
563 unique_lock<recursive_mutex> lock(sendMutex);
564 if(remoteAddress.sin_family != AF_INET) {
568 controlMsgData = protocol->getNextControlMessage(controlMsgLen);
570 if(controlMsgData !=
nullptr) {
571 sendNetworkMessage(controlMsgData, controlMsgLen);
578 int ImageTransfer::Pimpl::getNumDroppedFrames()
const {
579 return protocol->getNumDroppedFrames();
582 bool ImageTransfer::Pimpl::selectSocket(
bool read,
bool wait) {
585 unique_lock<recursive_mutex> lock(sendMutex);
600 if(select(sock+1, (read ? &fds :
nullptr), (!read ? &fds :
nullptr),
nullptr, &tv) <= 0) {
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.
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.
bool tryAccept()
Tries to accept a client connection.
void disconnect()
Terminates the current connection.
A lightweight protocol for transferring image pairs.
void setRawValidBytes(int validBytes)
Updates the number of valid bytes in a partial raw transmission.
No network connection has been established.
TransferStatus transferData()
Performs a partial (or full) image transmission.
ProtocolType
Supported network protocols.
bool isConnected() const
Returns true if a remote connection is established.
Aggregates information about a discovered device.
There is currently no more data that could be transmitted.
void setTransferImagePair(const ImagePair &imagePair)
Sets a new image pair that shall be transmitted.
The image pair has been transferred completely.
Exception class that is used for all transfer exceptions.
The connection oriented TCP transport protocol.
A set of two images, which are usually the left camera image and the disparity map.
TransferStatus
The result of a partial image transfer.
bool receiveImagePair(ImagePair &imagePair)
Waits for and receives a new image pair.