From d8f0dc25bdabef36b5d033a561bde0ff2c15c783 Mon Sep 17 00:00:00 2001
From: Ramon Wijnands <ramon.wijnands@nobleo.nl>
Date: Thu, 2 May 2024 15:07:50 +0200
Subject: [PATCH] Add parsing for all CAN flags

---
 CMakeLists.txt                                |  5 ++
 .../socketcan_bridge.hpp                      |  7 +++
 src/socketcan_bridge.cpp                      | 44 ++++++++++------
 test/unittests.cpp                            | 50 +++++++++++++++++++
 4 files changed, 92 insertions(+), 14 deletions(-)
 create mode 100644 test/unittests.cpp

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 92c03f4..4c74621 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -63,8 +63,13 @@ else()
 endif()
 
 if(BUILD_TESTING)
+  find_package(ament_cmake_gtest)
   find_package(ament_lint_auto REQUIRED)
+
   ament_lint_auto_find_test_dependencies()
+
+  ament_add_gtest(unittests test/unittests.cpp)
+  target_link_libraries(unittests socketcan_bridge_component)
 endif()
 
 ament_export_include_directories(
diff --git a/include/ros2_socketcan_bridge/socketcan_bridge.hpp b/include/ros2_socketcan_bridge/socketcan_bridge.hpp
index 91b5972..a7513c4 100644
--- a/include/ros2_socketcan_bridge/socketcan_bridge.hpp
+++ b/include/ros2_socketcan_bridge/socketcan_bridge.hpp
@@ -11,6 +11,8 @@
 #include "rclcpp/clock.hpp"
 #include "rclcpp/logger.hpp"
 
+// forward declarations
+struct can_frame;
 namespace rclcpp
 {
 class Node;
@@ -49,4 +51,9 @@ class SocketCanBridge
   std::jthread receive_thread_;
 };
 
+can_frame from_msg(const can_msgs::msg::Frame & msg);
+
+can_msgs::msg::Frame to_msg(const can_frame & frame);
+;
+
 }  // namespace ros2_socketcan_bridge
diff --git a/src/socketcan_bridge.cpp b/src/socketcan_bridge.cpp
index 81dfdd6..ba08ea3 100644
--- a/src/socketcan_bridge.cpp
+++ b/src/socketcan_bridge.cpp
@@ -6,7 +6,7 @@
 
 #include <fmt/core.h>
 #include <fmt/ostream.h>
-#include <linux/can/raw.h>
+#include <linux/can.h>
 #include <net/if.h>
 #include <sys/ioctl.h>
 #include <sys/socket.h>
@@ -65,13 +65,7 @@ SocketCanBridge::SocketCanBridge(
 
 void SocketCanBridge::send(const can_msgs::msg::Frame & msg)
 {
-  can_frame frame;
-  frame.can_id = msg.id;
-  frame.len = msg.dlc;
-  if (msg.is_extended) {
-    frame.can_id |= CAN_EFF_FLAG;
-  }
-  std::copy(msg.data.begin(), msg.data.end(), frame.data);
+  auto frame = from_msg(msg);
   RCLCPP_DEBUG_STREAM(logger_, "Sending " << msg);
   auto n = write(socket_, &frame, sizeof(frame));
   if (n < 0) {
@@ -107,13 +101,9 @@ void SocketCanBridge::receive_loop(std::stop_token stoken)
     if (nbytes != sizeof(frame)) {
       throw std::runtime_error("Partial CAN frame received");
     }
-    const bool ext = ((frame.can_id & CAN_EFF_FLAG) == CAN_EFF_FLAG);
-    can_msgs::msg::Frame msg;
+
+    auto msg = to_msg(frame);
     msg.header.stamp = clock_->now();
-    msg.id = frame.can_id & (ext ? CAN_EFF_MASK : CAN_SFF_MASK);
-    msg.dlc = frame.len;
-    msg.is_extended = ext;
-    std::copy_n(frame.data, sizeof(frame.data), msg.data.begin());
 
     RCLCPP_DEBUG_STREAM(logger_, "Received " << msg);
     receive_callback_(msg);
@@ -121,4 +111,30 @@ void SocketCanBridge::receive_loop(std::stop_token stoken)
   RCLCPP_INFO(logger_, "Receive loop stopped");
 }
 
+can_frame from_msg(const can_msgs::msg::Frame & msg)
+{
+  canid_t id = msg.id;
+  if (msg.is_rtr) id |= CAN_RTR_FLAG;
+  if (msg.is_extended) id |= CAN_EFF_FLAG;
+  if (msg.is_error) id |= CAN_ERR_FLAG;
+
+  can_frame frame;
+  frame.can_id = id;
+  frame.len = msg.dlc;
+  std::copy(msg.data.begin(), msg.data.end(), frame.data);
+  return frame;
+}
+
+can_msgs::msg::Frame to_msg(const can_frame & frame)
+{
+  can_msgs::msg::Frame msg;
+  msg.is_rtr = (frame.can_id & CAN_RTR_FLAG) == CAN_RTR_FLAG;
+  msg.is_extended = (frame.can_id & CAN_EFF_FLAG) == CAN_EFF_FLAG;
+  msg.is_error = (frame.can_id & CAN_ERR_FLAG) == CAN_ERR_FLAG;
+  msg.id = frame.can_id & (msg.is_extended ? CAN_EFF_MASK : CAN_SFF_MASK);
+  msg.dlc = frame.len;
+  std::copy_n(frame.data, sizeof(frame.data), msg.data.begin());
+  return msg;
+}
+
 }  // namespace ros2_socketcan_bridge
diff --git a/test/unittests.cpp b/test/unittests.cpp
new file mode 100644
index 0000000..d371c1b
--- /dev/null
+++ b/test/unittests.cpp
@@ -0,0 +1,50 @@
+// Copyright (C) 2024 Nobleo Technology B.V.
+//
+// SPDX-License-Identifier: Apache-2.0
+
+#include <gtest/gtest.h>
+#include <linux/can.h>
+
+#include "ros2_socketcan_bridge/socketcan_bridge.hpp"
+
+using ros2_socketcan_bridge::from_msg;
+using ros2_socketcan_bridge::to_msg;
+
+TEST(from_msg, standard)
+{
+  can_msgs::msg::Frame msg;
+  msg.id = 13;
+  auto frame = from_msg(msg);
+  EXPECT_EQ(frame.can_id, 13);
+}
+
+TEST(from_msg, extended)
+{
+  can_msgs::msg::Frame msg;
+  msg.id = 13;
+  msg.is_extended = true;
+  auto frame = from_msg(msg);
+  EXPECT_EQ(frame.can_id, 13 | CAN_EFF_FLAG);
+}
+
+TEST(to_msg, standard)
+{
+  struct can_frame frame;
+  frame.can_id = 13;
+  auto msg = to_msg(frame);
+  EXPECT_EQ(msg.id, 13);
+  EXPECT_FALSE(msg.is_rtr);
+  EXPECT_FALSE(msg.is_extended);
+  EXPECT_FALSE(msg.is_error);
+}
+
+TEST(to_msg, extended)
+{
+  struct can_frame frame;
+  frame.can_id = 13 | CAN_EFF_FLAG;
+  auto msg = to_msg(frame);
+  EXPECT_EQ(msg.id, 13);
+  EXPECT_FALSE(msg.is_rtr);
+  EXPECT_TRUE(msg.is_extended);
+  EXPECT_FALSE(msg.is_error);
+}