diff --git a/ros2_socketcan/src/socket_can_receiver.cpp b/ros2_socketcan/src/socket_can_receiver.cpp index f60d2df..09c71e1 100644 --- a/ros2_socketcan/src/socket_can_receiver.cpp +++ b/ros2_socketcan/src/socket_can_receiver.cpp @@ -31,6 +31,8 @@ #include #include +#define ERR_AGAIN_RETRY_CNT 10 + namespace drivers { namespace socketcan @@ -115,14 +117,21 @@ void SocketCanReceiver::wait(const std::chrono::nanoseconds timeout) const { if (decltype(timeout)::zero() < timeout) { auto c_timeout = to_timeval(timeout); + int retval; auto read_set = single_set(m_file_descriptor); - // Wait - if (0 == select(m_file_descriptor + 1, &read_set, NULL, NULL, &c_timeout)) { + retval = select(m_file_descriptor + 1, &read_set, NULL, NULL, &c_timeout); + // Check return value of the select function + if (0 == retval) { throw SocketCanTimeout{"CAN Receive Timeout"}; - } - //lint --e{9130, 1924, 9123, 9125, 1924, 9126} NOLINT - if (!FD_ISSET(m_file_descriptor, &read_set)) { - throw SocketCanTimeout{"CAN Receive timeout"}; + } else if (-1 == retval) { + // Output errno + throw std::runtime_error{strerror(errno)}; + } else { + // Check file descriptor + //lint --e{9130, 1924, 9123, 9125, 1924, 9126} NOLINT + if (0 == FD_ISSET(m_file_descriptor, &read_set)) { + throw SocketCanTimeout{"File descriptor not set"}; + } } } } @@ -137,16 +146,32 @@ CanId SocketCanReceiver::receive(void * const data, const std::chrono::nanosecon wait(timeout); // Read struct can_frame frame; - const auto nbytes = read(m_file_descriptor, &frame, sizeof(frame)); + + int number = 0; + ssize_t nbytes = 0; + + char buf[sizeof(frame)]; + char * p = &buf[0]; + ulong bytes_read = 0; + while (bytes_read < sizeof(frame) && -1 != nbytes) { + for (int i = 0; i < ERR_AGAIN_RETRY_CNT; i++) { + nbytes = read(m_file_descriptor, p + bytes_read, sizeof(frame) - bytes_read); + if (-1 == nbytes) { + number = errno; + if (!(EAGAIN == (number) || EWOULDBLOCK == (number) || EINTR == (number))) { + break; + } + } else { + bytes_read += nbytes; + break; + } + } + } // Checks - if (nbytes < 0) { + if (-1 == nbytes) { throw std::runtime_error{strerror(errno)}; - } - if (static_cast(nbytes) < sizeof(frame)) { - throw std::runtime_error{"read: incomplete CAN frame"}; - } - if (static_cast(nbytes) != sizeof(frame)) { - throw std::logic_error{"Message was wrong size"}; + } else { + (void)std::memcpy(&frame, &buf[0], sizeof(frame)); } // Write const auto data_length = static_cast(frame.can_dlc); diff --git a/ros2_socketcan/src/socket_can_sender.cpp b/ros2_socketcan/src/socket_can_sender.cpp index bf2cac3..ca7a10d 100644 --- a/ros2_socketcan/src/socket_can_sender.cpp +++ b/ros2_socketcan/src/socket_can_sender.cpp @@ -27,6 +27,8 @@ #include #include +#define ERR_AGAIN_RETRY_CNT 10 + namespace drivers { namespace socketcan @@ -105,14 +107,21 @@ void SocketCanSender::wait(const std::chrono::nanoseconds timeout) const { if (decltype(timeout)::zero() < timeout) { auto c_timeout = to_timeval(timeout); + int retval; auto write_set = single_set(m_file_descriptor); - // Wait - if (0 == select(m_file_descriptor + 1, NULL, &write_set, NULL, &c_timeout)) { + retval = select(m_file_descriptor + 1, NULL, &write_set, NULL, &c_timeout); + // Check return value of the select function + if (0 == retval) { throw SocketCanTimeout{"CAN Send Timeout"}; - } - //lint --e{9130, 9123, 9125, 1924, 9126} NOLINT - if (!FD_ISSET(m_file_descriptor, &write_set)) { - throw SocketCanTimeout{"CAN Send timeout"}; + } else if (-1 == retval) { + // Output errno + throw std::runtime_error{strerror(errno)}; + } else { + // Check file descriptor + //lint --e{9130, 1924, 9123, 9125, 1924, 9126} NOLINT + if (0 == FD_ISSET(m_file_descriptor, &write_set)) { + throw SocketCanTimeout{"File descriptor not set"}; + } } } } @@ -138,8 +147,30 @@ void SocketCanSender::send_impl( data_frame.can_dlc = static_cast(length); //lint -e{586} NOLINT data_frame is a stack variable; guaranteed not to overlap (void)std::memcpy(static_cast(&data_frame.data[0U]), data, length); - const auto bytes_sent = ::send(m_file_descriptor, &data_frame, sizeof(data_frame), flags); - if (0 > bytes_sent) { + + int number = 0; + ssize_t nbytes = 0; + + struct can_frame * p = &data_frame; + ulong bytes_sent = 0; + while (bytes_sent < sizeof(data_frame) && -1 != nbytes) { + for (int i = 0; i < ERR_AGAIN_RETRY_CNT; i++) { + nbytes = ::send( + m_file_descriptor, (char *)p + bytes_sent, sizeof(data_frame) - bytes_sent, + flags); + if (-1 == nbytes) { + number = errno; + if (!(EAGAIN == (number) || EWOULDBLOCK == (number) || EINTR == (number))) { + break; + } + } else { + bytes_sent += nbytes; + break; + } + } + } + // Checks + if (-1 == nbytes) { throw std::runtime_error{strerror(errno)}; } }