38 #include <sys/socket.h>
39 #include <sys/ioctl.h>
57 bool SocketCAN::open(
const std::string& interface, boost::function<
void(
const can_frame& frame)> handler,
62 sock_fd_ = socket(PF_CAN, SOCK_RAW, CAN_RAW);
65 ROS_ERROR(
"Error: Unable to create a CAN socket");
69 strncpy(name, interface.c_str(), interface.size());
74 ROS_ERROR(
"Unable to select CAN interface %s: I/O control error", name);
85 ROS_ERROR(
"Failed to bind socket to %s network interface", name);
132 int maxfd = sock->sock_fd_;
134 struct timeval timeout
138 can_frame rx_frame{};
140 sock->receiver_thread_running_ =
true;
141 while (!sock->terminate_receiver_thread_)
145 FD_ZERO(&descriptors);
147 FD_SET(sock->sock_fd_, &descriptors);
149 if (select(maxfd + 1, &descriptors,
nullptr,
nullptr, &timeout))
151 size_t len = read(sock->sock_fd_, &rx_frame, CAN_MTU);
154 if (sock->reception_handler !=
nullptr)
155 sock->reception_handler(rx_frame);
158 sock->receiver_thread_running_ =
false;
170 ROS_ERROR(
"Unable to start receiver thread");
174 sched_param sched{ .sched_priority = thread_priority };