Initial implementation

CMakeLists.txt

@@ -0,0 +1,37 @@
+cmake_minimum_required(VERSION 3.8)
+project(nobleo_socketcan_bridge)
+
+set(CMAKE_CXX_STANDARD_REQUIRED ON)
+set(CMAKE_CXX_EXTENSIONS OFF)
+set(CMAKE_CXX_STANDARD 20)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(ament_cmake_ros REQUIRED)
+find_package(can_msgs REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(fmt REQUIRED)
+
+add_executable(socketcan_bridge src/socketcan_bridge.cpp)
+target_include_directories(socketcan_bridge PUBLIC
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include>)
+ament_target_dependencies(socketcan_bridge
+  can_msgs
+  rclcpp
+)
+target_link_libraries(socketcan_bridge fmt::fmt)
+
+install(TARGETS socketcan_bridge
+  DESTINATION lib/${PROJECT_NAME})
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_package()

package.xml

@@ -0,0 +1,19 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>nobleo_socketcan_bridge</name>
+  <version>0.0.0</version>
+  <description>Simple wrapper around SocketCAN</description>
+  <author email="[email protected]">Ramon Wijnands</author>
+  <maintainer email="[email protected]">Ramon Wijnands</maintainer>
+  <license>TODO: License declaration</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

src/socketcan_bridge.cpp

@@ -0,0 +1,141 @@
+#include <fmt/core.h>
+#include <linux/can/raw.h>
+#include <net/if.h>
+#include <sys/ioctl.h>
+#include <sys/socket.h>
+
+#include <can_msgs/msg/frame.hpp>
+#include <rclcpp/experimental/executors/events_executor/events_executor.hpp>
+#include <thread>
+
+std::string to_string(const can_msgs::msg::Frame & msg)
+{
+  auto result = fmt::format("{:0>3X} [{}]", msg.id, msg.dlc);
+  for (auto i = 0; i < msg.dlc; ++i) {
+    result += fmt::format(" {:0>2X}", msg.data[i]);
+  }
+  return result;
+}
+
+class SocketCanBridge
+{
+public:
+  using CanCallback = std::function<void(const can_msgs::msg::Frame &)>;
+
+  SocketCanBridge(
+    rclcpp::Logger logger, rclcpp::Clock::SharedPtr clock, const std::string & interface,
+    const CanCallback & receive_callback)
+  : logger_(logger), clock_(clock), receive_callback_(receive_callback)
+  {
+    using std::placeholders::_1;
+
+    if ((socket_ = socket(PF_CAN, SOCK_RAW, CAN_RAW)) < 0) {
+      throw std::system_error(errno, std::generic_category());
+    }
+
+    timeval tv;
+    tv.tv_sec = 1;
+    tv.tv_usec = 0;
+    setsockopt(socket_, SOL_SOCKET, SO_RCVTIMEO, (const char *)&tv, sizeof tv);
+
+    ifreq ifr;
+    strcpy(ifr.ifr_name, interface.c_str());
+    ioctl(socket_, SIOCGIFINDEX, &ifr);
+
+    sockaddr_can addr;
+    memset(&addr, 0, sizeof(addr));
+    addr.can_family = AF_CAN;
+    addr.can_ifindex = ifr.ifr_ifindex;
+
+    if (bind(socket_, (sockaddr *)&addr, sizeof(addr)) < 0) {
+      throw std::system_error(errno, std::generic_category());
+    }
+
+    read_thread_ = std::jthread{std::bind(&SocketCanBridge::read_loop, this, _1)};
+  }
+
+  ~SocketCanBridge() { close(); }
+
+  void write(const can_msgs::msg::Frame & msg)
+  {
+    can_frame frame;
+    frame.can_id = msg.id;
+    frame.len = msg.dlc;
+    std::copy(msg.data.begin(), msg.data.end(), frame.data);
+    RCLCPP_INFO(logger_, "Sending %s", to_string(msg).c_str());
+    auto n = ::write(socket_, &frame, sizeof(frame));
+    if (n < 0) {
+      throw std::system_error(errno, std::generic_category());
+    }
+    RCLCPP_INFO(logger_, "Wrote %zd bytes to the socket", n);
+  }
+
+  void close()
+  {
+    RCLCPP_INFO(logger_, "Stopping the read thread");
+    read_thread_.request_stop();
+
+    RCLCPP_INFO(logger_, "Waiting for the read thread to stop");
+    read_thread_.join();
+
+    RCLCPP_INFO(logger_, "Closing the socket");
+    if (::close(socket_) < 0) {
+      throw std::system_error(errno, std::generic_category());
+    }
+  }
+
+private:
+  void read_loop(std::stop_token stoken)
+  {
+    RCLCPP_INFO(logger_, "Read loop started");
+    while (!stoken.stop_requested()) {
+      can_frame frame;
+      auto nbytes = read(socket_, &frame, sizeof(can_frame));
+      if (nbytes < 0) {
+        if (errno == EAGAIN) continue;
+        throw std::system_error(errno, std::generic_category());
+      }
+      if (nbytes != sizeof(frame)) {
+        throw std::runtime_error("Partial CAN frame received");
+      }
+
+      can_msgs::msg::Frame msg;
+      msg.header.stamp = clock_->now();
+      msg.id = frame.can_id;
+      msg.dlc = frame.len;
+      std::copy_n(frame.data, sizeof(frame.data), msg.data.begin());
+
+      RCLCPP_INFO(logger_, "Received %s", to_string(msg).c_str());
+      receive_callback_(msg);
+    }
+    RCLCPP_INFO(logger_, "Read loop stopped");
+  }
+
+  rclcpp::Logger logger_;
+  rclcpp::Clock::SharedPtr clock_;
+  int socket_;
+  CanCallback receive_callback_;
+  std::jthread read_thread_;
+};
+
+int main(int argc, char ** argv)
+{
+  rclcpp::init(argc, argv);
+
+  auto node = std::make_shared<rclcpp::Node>("socketcan_bridge");
+
+  auto can_pub = node->create_publisher<can_msgs::msg::Frame>("~/rx", 100);
+  SocketCanBridge::CanCallback cb = [&can_pub](const can_msgs::msg::Frame & msg) {
+    can_pub->publish(msg);
+  };
+
+  SocketCanBridge bridge{node->get_logger(), node->get_clock(), "vcan0", cb};
+  auto can_sub = node->create_subscription<can_msgs::msg::Frame>(
+    "~/tx", 100, [&bridge](can_msgs::msg::Frame::ConstSharedPtr msg) { bridge.write(*msg); });
+
+  rclcpp::experimental::executors::EventsExecutor executor;
+  executor.add_node(node);
+  executor.spin();
+
+  return EXIT_SUCCESS;
+}