fix(socket_can_receiver): support variable-length CAN FD frames (#50)

Signed-off-by: Kenzo Lobos-Tsunekawa <[email protected]> Co-authored-by: Max Schmeller <[email protected]> Co-authored-by: M. Fatih Cırıt <[email protected]>
autowarefoundation · Sep 25, 2024 · 85da8c3 · 85da8c3
1 parent d0a4422
commit 85da8c3
Show file tree

Hide file tree

Showing 2 changed files with 17 additions and 5 deletions.
diff --git a/ros2_socketcan/launch/socket_can_bridge.launch.xml b/ros2_socketcan/launch/socket_can_bridge.launch.xml
@@ -6,12 +6,14 @@
   <arg name="enable_can_fd" default="false" />
   <arg name="from_can_bus_topic" default="from_can_bus" />
   <arg name="to_can_bus_topic" default="to_can_bus" />
+  <arg name="use_bus_time" default="false" />
 
   <include file="$(find-pkg-share ros2_socketcan)/launch/socket_can_receiver.launch.py">
     <arg name="interface" value="$(var interface)" />
     <arg name="interval_sec" value="$(var receiver_interval_sec)" />
     <arg name="enable_can_fd" value="$(var enable_can_fd)" />
     <arg name="from_can_bus_topic" value="$(var from_can_bus_topic)" />
+    <arg name="use_bus_time" value="$(var use_bus_time)" />
   </include>
 
   <include file="$(find-pkg-share ros2_socketcan)/launch/socket_can_sender.launch.py">

diff --git a/ros2_socketcan/src/socket_can_receiver.cpp b/ros2_socketcan/src/socket_can_receiver.cpp
@@ -171,18 +171,28 @@ CanId SocketCanReceiver::receive_fd(void * const data, const std::chrono::nanose
   // Read
   struct canfd_frame frame;
   const auto nbytes = read(m_file_descriptor, &frame, sizeof(frame));
+
   // Checks
   if (nbytes < 0) {
     throw std::runtime_error{strerror(errno)};
   }
-  if (static_cast<std::size_t>(nbytes) < sizeof(frame)) {
-    throw std::runtime_error{"read: incomplete CAN FD frame"};
+
+  if (static_cast<std::size_t>(nbytes) < sizeof(frame.can_id) + sizeof(frame.len)) {
+    throw std::runtime_error{"read: corrupted CAN frame"};
   }
-  if (static_cast<std::size_t>(nbytes) != sizeof(frame)) {
-    throw std::logic_error{"Message was wrong size"};
+
+  if (frame.len > CANFD_MAX_DLEN) {
+    throw std::runtime_error{"read: frame length is larger than max allowed CAN FD payload length"};
   }
-  // Write
+
   const auto data_length = static_cast<CanId::LengthT>(frame.len);
+  // some CAN FD frames are shorter than 64 bytes
+  const auto expected_length = sizeof(frame) - sizeof(frame.data) + data_length;
+
+  if (static_cast<std::size_t>(nbytes) < expected_length) {
+    throw std::runtime_error{"read: incomplete CAN FD frame"};
+  }
+  // Write
   (void)std::memcpy(data, static_cast<void *>(&frame.data[0U]), data_length);
 
   // get bus timestamp