Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

feat: add error display and read/send iteration #31

Open
wants to merge 1 commit into
base: main
Choose a base branch
from
Open

feat: add error display and read/send iteration #31

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
53 changes: 39 additions & 14 deletions ros2_socketcan/src/socket_can_receiver.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -31,6 +31,8 @@
#include <vector>
#include <cstdio>

#define ERR_AGAIN_RETRY_CNT 10

namespace drivers
{
namespace socketcan
Expand Down Expand Up @@ -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"};
}
}
}
}
Expand All @@ -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<std::size_t>(nbytes) < sizeof(frame)) {
throw std::runtime_error{"read: incomplete CAN frame"};
}
if (static_cast<std::size_t>(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<CanId::LengthT>(frame.can_dlc);
Expand Down
47 changes: 39 additions & 8 deletions ros2_socketcan/src/socket_can_sender.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -27,6 +27,8 @@
#include <stdexcept>
#include <string>

#define ERR_AGAIN_RETRY_CNT 10

namespace drivers
{
namespace socketcan
Expand Down Expand Up @@ -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"};
}
}
}
}
Expand All @@ -138,8 +147,30 @@ void SocketCanSender::send_impl(
data_frame.can_dlc = static_cast<decltype(data_frame.can_dlc)>(length);
//lint -e{586} NOLINT data_frame is a stack variable; guaranteed not to overlap
(void)std::memcpy(static_cast<void *>(&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);
Comment on lines +158 to +160
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Suggested change
nbytes = ::send(
m_file_descriptor, (char *)p + bytes_sent, sizeof(data_frame) - bytes_sent,
flags);
nbytes = ::send(
m_file_descriptor, reinterpret_cast<char *>(p) + bytes_sent, sizeof(data_frame) - bytes_sent,
flags);

Related test error: https://github.com/autowarefoundation/ros2_socketcan/actions/runs/9974311030/job/27561524880?pr=31#step:5:1406

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)};
}
}
Expand Down
Loading