Initial implementation

.github/workflows/industrial_ci_action.yml

@@ -0,0 +1,36 @@
+# This config uses industrial_ci (https://github.com/ros-industrial/industrial_ci.git).
+# For troubleshooting, see README (https://github.com/ros-industrial/industrial_ci/blob/master/README.rst)
+
+name: CI
+
+on:
+  push:
+  pull_request:
+  workflow_dispatch:
+
+jobs:
+  industrial_ci:
+    name: ROS ${{ matrix.ROS_DISTRO }} (${{ matrix.ROS_REPO }})
+    runs-on: ubuntu-latest
+    strategy:
+      fail-fast: false
+      matrix:
+        ROS_DISTRO:
+          - humble
+          - iron
+        ROS_REPO:
+          - main
+    env:
+      CCACHE_DIR: "${{ github.workspace }}/.ccache"
+    steps:
+      - uses: actions/checkout@v3
+      - uses: actions/cache@v2
+        with:
+          path: ${{ env.CCACHE_DIR }}
+          key: ccache-${{ matrix.ROS_DISTRO }}-${{ matrix.ROS_REPO }}-${{github.run_id}}
+          restore-keys: |
+            ccache-${{ matrix.ROS_DISTRO }}-${{ matrix.ROS_REPO }}-
+      - uses: 'ros-industrial/industrial_ci@master'
+        env:
+          ROS_DISTRO: ${{ matrix.ROS_DISTRO }}
+          ROS_REPO: ${{ matrix.ROS_REPO }}

CMakeLists.txt

@@ -0,0 +1,63 @@
+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(fmt REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rclcpp_components REQUIRED)
+
+add_library(socketcan_bridge_component
+  src/socketcan_bridge.cpp
+  src/socketcan_bridge_node.cpp
+)
+target_include_directories(socketcan_bridge_component PUBLIC
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include>)
+ament_target_dependencies(socketcan_bridge_component
+  can_msgs
+  rclcpp
+  rclcpp_components
+)
+target_link_libraries(socketcan_bridge_component fmt::fmt)
+
+rclcpp_components_register_node(
+    socketcan_bridge_component
+    PLUGIN "nobleo_socketcan_bridge::SocketCanBridgeNode"
+    EXECUTABLE socketcan_bridge
+)
+
+install(
+  TARGETS socketcan_bridge_component
+  EXPORT export_${PROJECT_NAME}
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin
+)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_export_include_directories(
+  include
+)
+ament_export_libraries(
+  socketcan_bridge_component
+)
+ament_export_targets(
+  export_${PROJECT_NAME}
+)
+
+ament_package()

README.md

@@ -1,2 +1,33 @@
 # nobleo_socketcan_bridge
-Simple wrapper around SocketCAN
+The packages provides functionality to expose CAN frames from SocketCAN to a ROS Topic.
+
+## Overview
+Differences between this package and `ros2_socketcan`
+
+- No loopback, i.e. CAN frames that are send are not received by the same node
+- No lifecycle management, just run it
+- Less CPU usage
+
+## Nodes
+The main node (`socketcan_bridge`) is also available as dynamically loadable component.
+
+### socketcan_bridge
+
+#### Subscribed Topics
+
+* `~/tx` ([can_msgs/Frame])
+  Messages received here will be sent to the SocketCAN device.
+
+#### Published Topics
+* `~/rx` ([can_msgs/Frame])
+  Frames received on the SocketCAN device are published in this topic.
+
+#### Parameters
+
+* `interface`
+  Name of the SocketCAN device, by default these devices are named can0 and upwards.
+
+* `read_timeout`
+  Maximum duration to wait for data on the file descriptor
+
+[can_msgs/Frame]: https://github.com/ros-industrial/ros_canopen/blob/dashing-devel/can_msgs/msg/Frame.msg

include/nobleo_socketcan_bridge/socketcan_bridge.hpp

@@ -0,0 +1,44 @@
+#include <can_msgs/msg/frame.hpp>
+#include <rclcpp/clock.hpp>
+#include <rclcpp/logger.hpp>
+#include <thread>
+
+namespace rclcpp
+{
+class Node;
+class Clock;
+}  // namespace rclcpp
+
+namespace nobleo_socketcan_bridge
+{
+class SocketCanBridge
+{
+public:
+  using CanCallback = std::function<void(const can_msgs::msg::Frame &)>;
+
+  SocketCanBridge(
+    const rclcpp::Logger & logger, rclcpp::Clock::SharedPtr clock, const std::string & interface,
+    double read_timeout, const CanCallback & receive_callback);
+
+  ~SocketCanBridge() { close(); }
+
+  SocketCanBridge(const SocketCanBridge &) = delete;
+  SocketCanBridge(SocketCanBridge &&) noexcept = delete;
+  SocketCanBridge & operator=(const SocketCanBridge & other) = delete;
+  SocketCanBridge & operator=(SocketCanBridge && other) noexcept = delete;
+
+  void write(const can_msgs::msg::Frame & msg);
+
+  void close();
+
+private:
+  void read_loop(std::stop_token stoken);
+
+  rclcpp::Logger logger_;
+  rclcpp::Clock::SharedPtr clock_;
+  int socket_;
+  CanCallback receive_callback_;
+  std::jthread read_thread_;
+};
+
+}  // namespace nobleo_socketcan_bridge

include/nobleo_socketcan_bridge/socketcan_bridge_node.hpp

@@ -0,0 +1,17 @@
+#include <rclcpp/node.hpp>
+
+#include "nobleo_socketcan_bridge/socketcan_bridge.hpp"
+
+namespace nobleo_socketcan_bridge
+{
+class SocketCanBridgeNode : public rclcpp::Node
+{
+public:
+  SocketCanBridgeNode(const rclcpp::NodeOptions & options);
+
+private:
+  rclcpp::Publisher<can_msgs::msg::Frame>::SharedPtr can_pub;
+  SocketCanBridge bridge;
+  rclcpp::Subscription<can_msgs::msg::Frame>::SharedPtr can_sub;
+};
+}  // namespace nobleo_socketcan_bridge

package.xml

@@ -0,0 +1,33 @@
+<?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>
+  <maintainer email="[email protected]">Ramon Wijnands</maintainer>
+  <license>TODO: License declaration</license>
+  <author email="[email protected]">Ramon Wijnands</author>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>ament_cmake_ros</depend>
+  <depend>can_msgs</depend>
+  <depend>fmt</depend>
+  <depend>rclcpp</depend>
+  <depend>rclcpp_components</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+
+  <!-- Linters -->
+  <test_depend>ament_cmake_clang_format</test_depend>
+  <!-- <test_depend>ament_cmake_clang_tidy</test_depend> -->
+  <!-- <test_depend>ament_cmake_copyright</test_depend> -->
+  <test_depend>ament_cmake_cppcheck</test_depend>
+  <test_depend>ament_cmake_cpplint</test_depend>
+  <test_depend>ament_cmake_lint_cmake</test_depend>
+  <test_depend>ament_cmake_xmllint</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

src/socketcan_bridge.cpp

@@ -0,0 +1,113 @@
+#include "nobleo_socketcan_bridge/socketcan_bridge.hpp"
+
+#include <fmt/core.h>
+#include <linux/can/raw.h>
+#include <net/if.h>
+#include <sys/ioctl.h>
+#include <sys/socket.h>
+
+#include <cmath>
+#include <rclcpp/logging.hpp>
+
+namespace nobleo_socketcan_bridge
+{
+
+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;
+}
+
+SocketCanBridge::SocketCanBridge(
+  const rclcpp::Logger & logger, rclcpp::Clock::SharedPtr clock, const std::string & interface,
+  double read_timeout, 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());
+  }
+
+  auto microseconds = std::lround(read_timeout * 1'000'000);
+  timeval tv;
+  tv.tv_sec = microseconds / 1'000'000;
+  tv.tv_usec = microseconds - (tv.tv_sec * 1'000'000);
+  setsockopt(socket_, SOL_SOCKET, SO_RCVTIMEO, (const char *)&tv, sizeof tv);
+
+  ifreq ifr;
+  strncpy(ifr.ifr_name, interface.c_str(), sizeof(ifr.ifr_name));
+  if (ioctl(socket_, SIOCGIFINDEX, &ifr) < 0) {
+    throw std::system_error(errno, std::generic_category());
+  }
+
+  sockaddr_can addr;
+  memset(&addr, 0, sizeof(addr));
+  addr.can_family = AF_CAN;
+  addr.can_ifindex = ifr.ifr_ifindex;
+
+  if (bind(socket_, reinterpret_cast<sockaddr *>(&addr), sizeof(addr)) < 0) {
+    throw std::system_error(errno, std::generic_category());
+  }
+
+  read_thread_ = std::jthread{std::bind(&SocketCanBridge::read_loop, this, _1)};
+}
+
+void SocketCanBridge::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_DEBUG(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_DEBUG(logger_, "Wrote %zd bytes to the socket", n);
+}
+
+void SocketCanBridge::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());
+  }
+}
+
+void SocketCanBridge::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_DEBUG(logger_, "Received %s", to_string(msg).c_str());
+    receive_callback_(msg);
+  }
+  RCLCPP_INFO(logger_, "Read loop stopped");
+}
+
+}  // namespace nobleo_socketcan_bridge

src/socketcan_bridge_node.cpp

@@ -0,0 +1,21 @@
+#include "nobleo_socketcan_bridge/socketcan_bridge_node.hpp"
+
+#include <rclcpp_components/register_node_macro.hpp>
+
+namespace nobleo_socketcan_bridge
+{
+
+SocketCanBridgeNode::SocketCanBridgeNode(const rclcpp::NodeOptions & options)
+: rclcpp::Node("socketcan_bridge", options),
+  can_pub(this->create_publisher<can_msgs::msg::Frame>("~/rx", 100)),
+  bridge(
+    this->get_logger(), this->get_clock(), this->declare_parameter("interface", "can0"),
+    this->declare_parameter("read_timeout", 1.0),
+    [this](const can_msgs::msg::Frame & msg) { can_pub->publish(msg); }),
+  can_sub(this->create_subscription<can_msgs::msg::Frame>(
+    "~/tx", 100, [this](can_msgs::msg::Frame::ConstSharedPtr msg) { bridge.write(*msg); }))
+{
+}
+
+}  // namespace nobleo_socketcan_bridge
+RCLCPP_COMPONENTS_REGISTER_NODE(nobleo_socketcan_bridge::SocketCanBridgeNode)

test/benchmark.launch.xml

@@ -0,0 +1,15 @@
+<launch>
+    <arg name="rate" default="5000"/>
+
+    <executable cmd="ros2 topic pub -r $(var rate) -p $(var rate)0 /bridge1/tx can_msgs/msg/Frame '{}'"/>
+
+    <node name="bridge1" pkg="nobleo_socketcan_bridge" exec="socketcan_bridge">
+        <param name="interface" value="vcan0"/>
+    </node>
+
+    <node name="bridge2" pkg="nobleo_socketcan_bridge" exec="socketcan_bridge">
+        <param name="interface" value="vcan0"/>
+    </node>
+
+    <executable cmd="ros2 topic hz /bridge2/rx -w $(var rate)" output="screen"/>
+</launch>