socketcan.cpp
Go to the documentation of this file.
1 /*******************************************************************************
2  * BSD 3-Clause License
3  *
4  * Copyright (c) 2021, Qiayuan Liao
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions are met:
9  *
10  * * Redistributions of source code must retain the above copyright notice, this
11  * list of conditions and the following disclaimer.
12  *
13  * * Redistributions in binary form must reproduce the above copyright notice,
14  * this list of conditions and the following disclaimer in the documentation
15  * and/or other materials provided with the distribution.
16  *
17  * * Neither the name of the copyright holder nor the names of its
18  * contributors may be used to endorse or promote products derived from
19  * this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
22  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
23  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
24  * ARE
25  * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
26  * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
27  * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
28  * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
30  * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
31  * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
32  *******************************************************************************/
33 
34 //
35 // Created by qiayuan on 3/3/21.
36 //
38 #include <sys/socket.h>
39 #include <sys/ioctl.h>
40 #include <utility>
41 #include <ros/ros.h>
42 
43 namespace can
44 {
45 /* ref:
46  * https://github.com/JCube001/socketcan-demo
47  * http://blog.mbedded.ninja/programming/operating-systems/linux/how-to-use-socketcan-with-c-in-linux
48  * https://github.com/linux-can/can-utils/blob/master/candump.c
49  */
50 
52 {
53  if (this->isOpen())
54  this->close();
55 }
56 
57 bool SocketCAN::open(const std::string& interface, boost::function<void(const can_frame& frame)> handler,
58  int thread_priority)
59 {
60  reception_handler = std::move(handler);
61  // Request a socket
62  sock_fd_ = socket(PF_CAN, SOCK_RAW, CAN_RAW);
63  if (sock_fd_ == -1)
64  {
65  ROS_ERROR("Error: Unable to create a CAN socket");
66  return false;
67  }
68  char name[16] = {}; // avoid stringop-truncation
69  strncpy(name, interface.c_str(), interface.size());
70  strncpy(interface_request_.ifr_name, name, IFNAMSIZ);
71  // Get the index of the network interface
72  if (ioctl(sock_fd_, SIOCGIFINDEX, &interface_request_) == -1)
73  {
74  ROS_ERROR("Unable to select CAN interface %s: I/O control error", name);
75  // Invalidate unusable socket
76  close();
77  return false;
78  }
79  // Bind the socket to the network interface
80  address_.can_family = AF_CAN;
81  address_.can_ifindex = interface_request_.ifr_ifindex;
82  int rc = bind(sock_fd_, reinterpret_cast<struct sockaddr*>(&address_), sizeof(address_));
83  if (rc == -1)
84  {
85  ROS_ERROR("Failed to bind socket to %s network interface", name);
86  close();
87  return false;
88  }
89  // Start a separate, event-driven thread for frame reception
90  return startReceiverThread(thread_priority);
91 }
92 
93 void SocketCAN::close()
94 {
97  ;
98 
99  if (!isOpen())
100  return;
101  // Close the file descriptor for our socket
102  ::close(sock_fd_);
103  sock_fd_ = -1;
104 }
105 
106 bool SocketCAN::isOpen() const
107 {
108  return (sock_fd_ != -1);
109 }
110 
111 void SocketCAN::write(can_frame* frame) const
112 {
113  if (!isOpen())
114  {
115  ROS_ERROR_THROTTLE(5., "Unable to write: Socket %s not open", interface_request_.ifr_name);
116  return;
117  }
118  if (::write(sock_fd_, frame, sizeof(can_frame)) == -1)
119  ROS_DEBUG_THROTTLE(5., "Unable to write: The %s tx buffer may be full", interface_request_.ifr_name);
120 }
121 
122 static void* socketcan_receiver_thread(void* argv)
123 {
124  /*
125  * The first and only argument to this function
126  * is the pointer to the object, which started the thread.
127  */
128  auto* sock = (SocketCAN*)argv;
129  // Holds the set of descriptors, that 'select' shall monitor
130  fd_set descriptors;
131  // Highest file descriptor in set
132  int maxfd = sock->sock_fd_;
133  // How long 'select' shall wait before returning with timeout
134  struct timeval timeout
135  {
136  };
137  // Buffer to store incoming frame
138  can_frame rx_frame{};
139  // Run until termination signal received
140  sock->receiver_thread_running_ = true;
141  while (!sock->terminate_receiver_thread_)
142  {
143  timeout.tv_sec = 1.; // Should be set each loop
144  // Clear descriptor set
145  FD_ZERO(&descriptors);
146  // Add socket descriptor
147  FD_SET(sock->sock_fd_, &descriptors);
148  // Wait until timeout or activity on any descriptor
149  if (select(maxfd + 1, &descriptors, nullptr, nullptr, &timeout))
150  {
151  size_t len = read(sock->sock_fd_, &rx_frame, CAN_MTU);
152  if (len < 0)
153  continue;
154  if (sock->reception_handler != nullptr)
155  sock->reception_handler(rx_frame);
156  }
157  }
158  sock->receiver_thread_running_ = false;
159  return nullptr;
160 }
161 
162 bool SocketCAN::startReceiverThread(int thread_priority)
163 {
164  // Frame reception is accomplished in a separate, event-driven thread.
165  // See also: https://www.thegeekstuff.com/2012/04/create-threads-in-linux/
167  int rc = pthread_create(&receiver_thread_id_, nullptr, &socketcan_receiver_thread, this);
168  if (rc != 0)
169  {
170  ROS_ERROR("Unable to start receiver thread");
171  return false;
172  }
173  ROS_INFO("Successfully started receiver thread with ID %lu", receiver_thread_id_);
174  sched_param sched{ .sched_priority = thread_priority };
175  pthread_setschedparam(receiver_thread_id_, SCHED_FIFO, &sched);
176  return true;
177 }
178 
179 } // namespace can
ROS_ERROR_THROTTLE
#define ROS_ERROR_THROTTLE(period,...)
can::SocketCAN::open
bool open(const std::string &interface, boost::function< void(const can_frame &frame)> handler, int thread_priority)
Open and bind socket.
Definition: socketcan.cpp:88
can::SocketCAN::isOpen
bool isOpen() const
Returns whether the socket is open or closed.
Definition: socketcan.cpp:137
can::socketcan_receiver_thread
static void * socketcan_receiver_thread(void *argv)
Definition: socketcan.cpp:153
ros.h
can::SocketCAN::receiver_thread_running_
bool receiver_thread_running_
Definition: socketcan.h:126
can::SocketCAN::~SocketCAN
~SocketCAN()
Definition: socketcan.cpp:82
can::SocketCAN::interface_request_
ifreq interface_request_
Definition: socketcan.h:113
can::SocketCAN::startReceiverThread
bool startReceiverThread(int thread_priority)
Starts a new thread, that will wait for socket events.
Definition: socketcan.cpp:193
can::SocketCAN::sock_fd_
int sock_fd_
Definition: socketcan.h:121
can::SocketCAN::reception_handler
boost::function< void(const can_frame &frame)> reception_handler
Definition: socketcan.h:161
can
Definition: socketcan.h:46
ROS_ERROR
#define ROS_ERROR(...)
can::SocketCAN::close
void close()
Close and unbind socket.
Definition: socketcan.cpp:124
can::SocketCAN
Definition: socketcan.h:79
ROS_INFO
#define ROS_INFO(...)
can::SocketCAN::write
void write(can_frame *frame) const
Sends the referenced frame to the bus.
Definition: socketcan.cpp:142
can::SocketCAN::terminate_receiver_thread_
bool terminate_receiver_thread_
Definition: socketcan.h:125
ROS_DEBUG_THROTTLE
#define ROS_DEBUG_THROTTLE(period,...)
socketcan.h
can::SocketCAN::address_
sockaddr_can address_
Definition: socketcan.h:114
can::SocketCAN::receiver_thread_id_
pthread_t receiver_thread_id_
Definition: socketcan.h:115


rm_hw
Author(s): Qiayuan Liao
autogenerated on Tue May 6 2025 02:23:44