int32_t TransportTCP::write(uint8_t* buffer, uint32_t size)
{
{
boost::recursive_mutex::scoped_lock lock(close_mutex_);
if (closed_)
{
ROSCPP_LOG_DEBUG("Tried to write on a closed socket [%d]", sock_);
return -1;
}
}
ROS_ASSERT((int32_t)size > 0);
int num_bytes = ::send(sock_, reinterpret_cast<const char*>(buffer), size, 0);
if (num_bytes < 0)
{
if ( !last_socket_error_is_would_block() )
{
ROSCPP_LOG_DEBUG("send() on socket [%d] failed with error [%s]", sock_, last_socket_error_string());
close();
}
else
{
num_bytes = 0;
}
}
return num_bytes;
}
int32_t TransportTCP::write(uint8_t* buffer, uint32_t size)
{
{
boost::recursive_mutex::scoped_lock lock(close_mutex_);
if (closed_)
{
ROSCPP_LOG_DEBUG("Tried to write on a closed socket [%d]", sock_);
return -1;
}
}
ROS_ASSERT(size > 0);
// never write more than INT_MAX since this is the maximum we can report back with the current return type
uint32_t writesize = std::min(size, static_cast<uint32_t>(INT_MAX));
int num_bytes = ::send(sock_, reinterpret_cast<const char*>(buffer), writesize, 0);
if (num_bytes < 0)
{
if ( !last_socket_error_is_would_block() )
{
ROSCPP_LOG_DEBUG("send() on socket [%d] failed with error [%s]", sock_, last_socket_error_string());
close();
}
else
{
num_bytes = 0;
}
}
return num_bytes;
}
int32_t TransportTCP::read(uint8_t* buffer, uint32_t size)
{
{
boost::recursive_mutex::scoped_lock lock(close_mutex_);
if (closed_)
{
ROSCPP_LOG_DEBUG("Tried to read on a closed socket [%d]", sock_);
return -1;
}
}
ROS_ASSERT((int32_t)size > 0);
int num_bytes = ::recv(sock_, reinterpret_cast<char*>(buffer), size, 0);
if (num_bytes < 0)
{
if ( !last_socket_error_is_would_block() ) // !WSAWOULDBLOCK / !EAGAIN && !EWOULDBLOCK
{
ROSCPP_LOG_DEBUG("recv() on socket [%d] failed with error [%s]", sock_, last_socket_error_string());
close();
}
else
{
num_bytes = 0;
}
}
else if (num_bytes == 0)
{
ROSCPP_LOG_DEBUG("Socket [%d] received 0/%d bytes, closing", sock_, size);
close();
return -1;
}
return num_bytes;
}
int32_t TransportTCP::read(uint8_t* buffer, uint32_t size)
{
{
boost::recursive_mutex::scoped_lock lock(close_mutex_);
if (closed_)
{
ROSCPP_LOG_DEBUG("Tried to read on a closed socket [%d]", sock_);
return -1;
}
}
ROS_ASSERT(size > 0);
// never read more than INT_MAX since this is the maximum we can report back with the current return type
uint32_t read_size = std::min(size, static_cast<uint32_t>(INT_MAX));
int num_bytes = ::recv(sock_, reinterpret_cast<char*>(buffer), read_size, 0);
if (num_bytes < 0)
{
if ( !last_socket_error_is_would_block() ) // !WSAWOULDBLOCK / !EAGAIN && !EWOULDBLOCK
{
ROSCPP_LOG_DEBUG("recv() on socket [%d] failed with error [%s]", sock_, last_socket_error_string());
close();
}
else
{
num_bytes = 0;
}
}
else if (num_bytes == 0)
{
ROSCPP_LOG_DEBUG("Socket [%d] received 0/%u bytes, closing", sock_, size);
close();
return -1;
}
return num_bytes;
}
int32_t TransportUDP::write(uint8_t* buffer, uint32_t size)
{
{
boost::mutex::scoped_lock lock(close_mutex_);
if (closed_)
{
ROSCPP_LOG_DEBUG("Tried to write on a closed socket [%d]", sock_);
return -1;
}
}
ROS_ASSERT((int32_t)size > 0);
const uint32_t max_payload_size = max_datagram_size_ - sizeof(TransportUDPHeader);
uint32_t bytes_sent = 0;
uint32_t this_block = 0;
if (++current_message_id_ == 0)
++current_message_id_;
while (bytes_sent < size)
{
TransportUDPHeader header;
header.connection_id_ = connection_id_;
header.message_id_ = current_message_id_;
if (this_block == 0)
{
header.op_ = ROS_UDP_DATA0;
header.block_ = (size + max_payload_size - 1) / max_payload_size;
}
else
{
header.op_ = ROS_UDP_DATAN;
header.block_ = this_block;
}
++this_block;
#if defined(WIN32)
WSABUF iov[2];
iov[0].buf = reinterpret_cast<char*>(&header);
iov[0].len = sizeof(header);
iov[1].buf = reinterpret_cast<char*>(buffer + bytes_sent);
iov[1].len = std::min(max_payload_size, size - bytes_sent);
ssize_t num_bytes;
if (WSASend(sock_, iov, 2, reinterpret_cast<LPDWORD>(&num_bytes), 0, NULL,
NULL) == SOCKET_ERROR) {
num_bytes = -1;
}
#else
struct iovec iov[2];
iov[0].iov_base = &header;
iov[0].iov_len = sizeof(header);
iov[1].iov_base = buffer + bytes_sent;
iov[1].iov_len = std::min(max_payload_size, size - bytes_sent);
ssize_t num_bytes = writev(sock_, iov, 2);
#endif
//usleep(100);
if (num_bytes < 0)
{
if( last_socket_error_is_would_block() )
{
ROSCPP_LOG_DEBUG("writev() failed with error [%s]", last_socket_error_string());
close();
break;
}
else
{
num_bytes = 0;
}
}
else if (num_bytes < ssize_t(sizeof(header)))
{
ROSCPP_LOG_DEBUG("Socket [%d] short write (%d bytes), closing", sock_, int(num_bytes));
close();
break;
}
else
{
num_bytes -= sizeof(header);
}
bytes_sent += num_bytes;
}
return bytes_sent;
}
int32_t TransportUDP::read(uint8_t* buffer, uint32_t size)
{
{
boost::mutex::scoped_lock lock(close_mutex_);
if (closed_)
{
ROSCPP_LOG_DEBUG("Tried to read on a closed socket [%d]", sock_);
return -1;
}
}
ROS_ASSERT((int32_t)size > 0);
uint32_t bytes_read = 0;
while (bytes_read < size)
{
TransportUDPHeader header;
ssize_t num_bytes = 0;
bool from_previous = false;
if (reorder_bytes_)
{
if (reorder_start_ != reorder_buffer_)
{
from_previous = true;
}
num_bytes = std::min(size - bytes_read, reorder_bytes_);
header = reorder_header_;
memcpy(buffer + bytes_read, reorder_start_, num_bytes);
reorder_bytes_ -= num_bytes;
reorder_start_ += num_bytes;
}
else
{
if (data_filled_ == 0)
{
#if defined(WIN32)
WSABUF iov[2];
iov[0].buf = reinterpret_cast<char*>(&header);
iov[0].len = sizeof(header);
iov[1].buf = reinterpret_cast<char*>(data_buffer_);
iov[1].len = max_datagram_size_ - sizeof(header);
if (WSARecv(sock_, iov, 2, reinterpret_cast<LPDWORD>(&num_bytes), 0, NULL, NULL) == SOCKET_ERROR) {
num_bytes = -1;
}
#else
struct iovec iov[2];
iov[0].iov_base = &header;
iov[0].iov_len = sizeof(header);
iov[1].iov_base = data_buffer_;
iov[1].iov_len = max_datagram_size_ - sizeof(header);
// Read a datagram with header
num_bytes = readv(sock_, iov, 2);
#endif
if (num_bytes < 0)
{
if ( last_socket_error_is_would_block() )
{
num_bytes = 0;
break;
}
else
{
ROSCPP_LOG_DEBUG("readv() failed with error [%s]", last_socket_error_string());
close();
break;
}
}
else if (num_bytes == 0)
{
ROSCPP_LOG_DEBUG("Socket [%d] received 0/%d bytes, closing", sock_, size);
close();
return -1;
}
else if (num_bytes < (ssize_t)sizeof(header))
{
ROS_ERROR("Socket [%d] received short header (%d bytes): %s", sock_, int(num_bytes), last_socket_error_string());
close();
return -1;
}
num_bytes -= sizeof(header);
data_filled_ = num_bytes;
data_start_ = data_buffer_;
}
else
{
from_previous = true;
}
num_bytes = std::min(size - bytes_read, data_filled_);
// Copy from the data buffer, whether it has data left in it from a previous datagram or
// was just filled by readv()
memcpy(buffer + bytes_read, data_start_, num_bytes);
data_filled_ = std::max((int64_t)0, (int64_t)data_filled_ - (int64_t)size);
data_start_ += num_bytes;
}
if (from_previous)
{
bytes_read += num_bytes;
}
else
{
// Process header
switch (header.op_)
{
case ROS_UDP_DATA0:
if (current_message_id_)
{
ROS_DEBUG("Received new message [%d:%d], while still working on [%d] (block %d of %d)", header.message_id_, header.block_, current_message_id_, last_block_ + 1, total_blocks_);
reorder_header_ = header;
reorder_bytes_ = num_bytes;
memcpy(reorder_buffer_, buffer + bytes_read, num_bytes);
reorder_start_ = reorder_buffer_;
current_message_id_ = 0;
total_blocks_ = 0;
last_block_ = 0;
data_filled_ = 0;
data_start_ = data_buffer_;
return -1;
}
total_blocks_ = header.block_;
last_block_ = 0;
current_message_id_ = header.message_id_;
break;
case ROS_UDP_DATAN:
if (header.message_id_ != current_message_id_)
{
ROS_DEBUG("Message Id mismatch: %d != %d", header.message_id_, current_message_id_);
return 0;
}
if (header.block_ != last_block_ + 1)
{
ROS_DEBUG("Expected block %d, received %d", last_block_ + 1, header.block_);
return 0;
}
last_block_ = header.block_;
break;
default:
ROS_ERROR("Unexpected UDP header OP [%d]", header.op_);
return -1;
}
bytes_read += num_bytes;
if (last_block_ == (total_blocks_ - 1))
{
current_message_id_ = 0;
break;
}
}
}
return bytes_read;
}
int32_t TransportUDP::read(uint8_t* buffer, uint32_t size)
{
{
boost::mutex::scoped_lock lock(close_mutex_);
if (closed_)
{
ROSCPP_LOG_DEBUG("Tried to read on a closed socket [%d]", sock_);
return -1;
}
}
ROS_ASSERT((int32_t)size > 0);
uint32_t bytes_read = 0;
while (bytes_read < size)
{
TransportUDPHeader header;
// Get the data either from the reorder buffer or the socket
// copy_bytes will contain the read size.
// from_previous is true if the data belongs to the previous UDP datagram.
uint32_t copy_bytes = 0;
bool from_previous = false;
if (reorder_bytes_)
{
if (reorder_start_ != reorder_buffer_)
{
from_previous = true;
}
copy_bytes = std::min(size - bytes_read, reorder_bytes_);
header = reorder_header_;
memcpy(buffer + bytes_read, reorder_start_, copy_bytes);
reorder_bytes_ -= copy_bytes;
reorder_start_ += copy_bytes;
}
else
{
if (data_filled_ == 0)
{
#if defined(WIN32)
SSIZE_T num_bytes = 0;
DWORD received_bytes = 0;
DWORD flags = 0;
WSABUF iov[2];
iov[0].buf = reinterpret_cast<char*>(&header);
iov[0].len = sizeof(header);
iov[1].buf = reinterpret_cast<char*>(data_buffer_);
iov[1].len = max_datagram_size_ - sizeof(header);
int rc = WSARecv(sock_, iov, 2, &received_bytes, &flags, NULL, NULL);
if ( rc == SOCKET_ERROR) {
num_bytes = -1;
} else {
num_bytes = received_bytes;
}
#else
ssize_t num_bytes;
struct iovec iov[2];
iov[0].iov_base = &header;
iov[0].iov_len = sizeof(header);
iov[1].iov_base = data_buffer_;
iov[1].iov_len = max_datagram_size_ - sizeof(header);
// Read a datagram with header
num_bytes = readv(sock_, iov, 2);
#endif
if (num_bytes < 0)
{
if ( last_socket_error_is_would_block() )
{
num_bytes = 0;
break;
}
else
{
ROSCPP_LOG_DEBUG("readv() failed with error [%s]", last_socket_error_string());
close();
break;
}
}
else if (num_bytes == 0)
{
ROSCPP_LOG_DEBUG("Socket [%d] received 0/%d bytes, closing", sock_, size);
close();
return -1;
}
else if (num_bytes < (unsigned) sizeof(header))
{
ROS_ERROR("Socket [%d] received short header (%d bytes): %s", sock_, int(num_bytes), last_socket_error_string());
close();
return -1;
}
num_bytes -= sizeof(header);
data_filled_ = num_bytes;
data_start_ = data_buffer_;
}
else
{
from_previous = true;
}
copy_bytes = std::min(size - bytes_read, data_filled_);
// Copy from the data buffer, whether it has data left in it from a previous datagram or
// was just filled by readv()
memcpy(buffer + bytes_read, data_start_, copy_bytes);
data_filled_ -= copy_bytes;
data_start_ += copy_bytes;
}
if (from_previous)
{
// We are simply reading data from the last UDP datagram, nothing to
// parse
bytes_read += copy_bytes;
}
else
{
// This datagram is new, process header
switch (header.op_)
{
case ROS_UDP_DATA0:
if (current_message_id_)
{
ROS_DEBUG("Received new message [%d:%d], while still working on [%d] (block %d of %d)", header.message_id_, header.block_, current_message_id_, last_block_ + 1, total_blocks_);
reorder_header_ = header;
// Copy the entire data buffer to the reorder buffer, as we will
// need to replay this UDP datagram in the next call.
reorder_bytes_ = data_filled_ + (data_start_ - data_buffer_);
memcpy(reorder_buffer_, data_buffer_, reorder_bytes_);
reorder_start_ = reorder_buffer_;
current_message_id_ = 0;
total_blocks_ = 0;
last_block_ = 0;
data_filled_ = 0;
data_start_ = data_buffer_;
return -1;
}
total_blocks_ = header.block_;
last_block_ = 0;
current_message_id_ = header.message_id_;
break;
case ROS_UDP_DATAN:
if (header.message_id_ != current_message_id_)
{
ROS_DEBUG("Message Id mismatch: %d != %d", header.message_id_, current_message_id_);
data_filled_ = 0; // discard datagram
return 0;
}
if (header.block_ != last_block_ + 1)
{
ROS_DEBUG("Expected block %d, received %d", last_block_ + 1, header.block_);
data_filled_ = 0; // discard datagram
return 0;
}
last_block_ = header.block_;
break;
default:
ROS_ERROR("Unexpected UDP header OP [%d]", header.op_);
return -1;
}
bytes_read += copy_bytes;
if (last_block_ == (total_blocks_ - 1))
{
current_message_id_ = 0;
break;
}
}
}
return bytes_read;
}
