diff --git a/.github/workflows/industrial_ci_action.yml b/.github/workflows/industrial_ci_action.yml new file mode 100644 index 0000000..2bafeab --- /dev/null +++ b/.github/workflows/industrial_ci_action.yml @@ -0,0 +1,40 @@ +# Copyright (C) 2024 Nobleo Technology B.V. +# +# SPDX-License-Identifier: Apache-2.0 + +# 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 }} diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..92c03f4 --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,80 @@ +# Copyright (C) 2024 Nobleo Technology B.V. +# +# SPDX-License-Identifier: Apache-2.0 + +cmake_minimum_required(VERSION 3.8) +project(ros2_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 + $ + $) +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 "ros2_socketcan_bridge::SocketCanBridgeNode" + EXECUTABLE socketcan_bridge +) + +install( + TARGETS socketcan_bridge_component + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +# EventsExecutor is supported from rclcpp version 21 or greater (iron) +if(rclcpp_VERSION_MAJOR GREATER_EQUAL 21) + message(STATUS "EventsExecutor support enabled") + add_executable(socketcan_bridge_ee src/socketcan_bridge_ee.cpp) + target_link_libraries(socketcan_bridge_ee socketcan_bridge_component) + + install(TARGETS socketcan_bridge_ee + DESTINATION lib/${PROJECT_NAME} + ) +else() + message(STATUS "EventsExecutor support disabled") +endif() + +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() diff --git a/README.md b/README.md index a410e35..fa0a435 100644 --- a/README.md +++ b/README.md @@ -1,2 +1,49 @@ -# nobleo_socketcan_bridge -Simple wrapper around SocketCAN + + +# ros2_socketcan_bridge + +This package provides functionality to expose CAN frames from SocketCAN to ROS2 topics. + +## Overview + +This is a from-scratch re-implementation of [socketcan_bridge] from ROS1. +There is a different ROS2 package [ros2_socketcan] which is similar to this package. +The differences between this package and [ros2_socketcan] are: + +- 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. +The node `socketcan_bridge_ee` is also provided that uses the `EventsExecutor` that runs a bit more efficient. + +### 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 on 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 + +[socketcan_bridge]: https://wiki.ros.org/socketcan_bridge + +[ros2_socketcan]: https://github.com/autowarefoundation/ros2_socketcan \ No newline at end of file diff --git a/include/ros2_socketcan_bridge/socketcan_bridge.hpp b/include/ros2_socketcan_bridge/socketcan_bridge.hpp new file mode 100644 index 0000000..91b5972 --- /dev/null +++ b/include/ros2_socketcan_bridge/socketcan_bridge.hpp @@ -0,0 +1,52 @@ +// Copyright (C) 2024 Nobleo Technology B.V. +// +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include +#include + +#include "can_msgs/msg/frame.hpp" +#include "rclcpp/clock.hpp" +#include "rclcpp/logger.hpp" + +namespace rclcpp +{ +class Node; +class Clock; +} // namespace rclcpp + +namespace ros2_socketcan_bridge +{ +class SocketCanBridge +{ +public: + using CanCallback = std::function; + + 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 send(const can_msgs::msg::Frame & msg); + + void close(); + +private: + void receive_loop(std::stop_token stoken); + + rclcpp::Logger logger_; + rclcpp::Clock::SharedPtr clock_; + int socket_; + CanCallback receive_callback_; + std::jthread receive_thread_; +}; + +} // namespace ros2_socketcan_bridge diff --git a/include/ros2_socketcan_bridge/socketcan_bridge_node.hpp b/include/ros2_socketcan_bridge/socketcan_bridge_node.hpp new file mode 100644 index 0000000..170a04e --- /dev/null +++ b/include/ros2_socketcan_bridge/socketcan_bridge_node.hpp @@ -0,0 +1,22 @@ +// Copyright (C) 2024 Nobleo Technology B.V. +// +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include "rclcpp/node.hpp" +#include "ros2_socketcan_bridge/socketcan_bridge.hpp" + +namespace ros2_socketcan_bridge +{ +class SocketCanBridgeNode : public rclcpp::Node +{ +public: + explicit SocketCanBridgeNode(const rclcpp::NodeOptions & options); + +private: + rclcpp::Publisher::SharedPtr can_pub; + SocketCanBridge bridge; + rclcpp::Subscription::SharedPtr can_sub; +}; +} // namespace ros2_socketcan_bridge diff --git a/package.xml b/package.xml new file mode 100644 index 0000000..be6a39b --- /dev/null +++ b/package.xml @@ -0,0 +1,40 @@ + + + + + + + ros2_socketcan_bridge + 0.0.0 + Simple wrapper around SocketCAN + Ramon Wijnands + TODO: License declaration + Ramon Wijnands + + ament_cmake + + ament_cmake_ros + can_msgs + fmt + rclcpp + rclcpp_components + + ament_lint_auto + + + ament_cmake_clang_format + + + ament_cmake_cppcheck + ament_cmake_cpplint + ament_cmake_lint_cmake + ament_cmake_xmllint + + + ament_cmake + + diff --git a/src/socketcan_bridge.cpp b/src/socketcan_bridge.cpp new file mode 100644 index 0000000..1daa923 --- /dev/null +++ b/src/socketcan_bridge.cpp @@ -0,0 +1,120 @@ +// Copyright (C) 2024 Nobleo Technology B.V. +// +// SPDX-License-Identifier: Apache-2.0 + +#include "ros2_socketcan_bridge/socketcan_bridge.hpp" + +#include +#include +#include +#include +#include +#include + +#include + +#include "rclcpp/logging.hpp" + +namespace ros2_socketcan_bridge +{ +std::ostream & operator<<(std::ostream & os, const can_msgs::msg::Frame & msg) +{ + fmt::print(os, "{:0>3X} [{}]", msg.id, msg.dlc); + for (auto i = 0; i < msg.dlc; ++i) { + fmt::print(os, " {:0>2X}", msg.data[i]); + } + return os; +} + +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()); + } + + // When SIGINT is received, the program needs to quit. But a `read()` call can't be interrupted. + // So I use SO_RCVTIMEO to make all reads return within `read_timeout` seconds. + 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(&addr), sizeof(addr)) < 0) { + throw std::system_error(errno, std::generic_category()); + } + + receive_thread_ = std::jthread{std::bind(&SocketCanBridge::receive_loop, this, _1)}; +} + +void SocketCanBridge::send(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_STREAM(logger_, "Sending " << msg); + 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 receive thread"); + receive_thread_.request_stop(); + + RCLCPP_INFO(logger_, "Waiting for the receive thread to stop"); + receive_thread_.join(); + + RCLCPP_INFO(logger_, "Closing the socket"); + if (::close(socket_) < 0) { + throw std::system_error(errno, std::generic_category()); + } +} + +void SocketCanBridge::receive_loop(std::stop_token stoken) +{ + RCLCPP_INFO(logger_, "Receive 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_STREAM(logger_, "Received " << msg); + receive_callback_(msg); + } + RCLCPP_INFO(logger_, "Receive loop stopped"); +} + +} // namespace ros2_socketcan_bridge diff --git a/src/socketcan_bridge_ee.cpp b/src/socketcan_bridge_ee.cpp new file mode 100644 index 0000000..e5c9f6d --- /dev/null +++ b/src/socketcan_bridge_ee.cpp @@ -0,0 +1,19 @@ +// Copyright (C) 2024 Nobleo Technology B.V. +// +// SPDX-License-Identifier: Apache-2.0 + +#include "rclcpp/experimental/executors/events_executor/events_executor.hpp" +#include "ros2_socketcan_bridge/socketcan_bridge_node.hpp" + +using ros2_socketcan_bridge::SocketCanBridgeNode; + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + auto node = std::make_shared(rclcpp::NodeOptions{}); + rclcpp::experimental::executors::EventsExecutor executor; + executor.add_node(node); + executor.spin(); + rclcpp::shutdown(); + return EXIT_SUCCESS; +} diff --git a/src/socketcan_bridge_node.cpp b/src/socketcan_bridge_node.cpp new file mode 100644 index 0000000..50c4a56 --- /dev/null +++ b/src/socketcan_bridge_node.cpp @@ -0,0 +1,25 @@ +// Copyright (C) 2024 Nobleo Technology B.V. +// +// SPDX-License-Identifier: Apache-2.0 + +#include "ros2_socketcan_bridge/socketcan_bridge_node.hpp" + +#include + +namespace ros2_socketcan_bridge +{ + +SocketCanBridgeNode::SocketCanBridgeNode(const rclcpp::NodeOptions & options) +: rclcpp::Node("socketcan_bridge", options), + can_pub(this->create_publisher("~/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( + "~/tx", 100, [this](can_msgs::msg::Frame::ConstSharedPtr msg) { bridge.send(*msg); })) +{ +} + +} // namespace ros2_socketcan_bridge +RCLCPP_COMPONENTS_REGISTER_NODE(ros2_socketcan_bridge::SocketCanBridgeNode) diff --git a/test/benchmark.launch.xml b/test/benchmark.launch.xml new file mode 100644 index 0000000..c748e27 --- /dev/null +++ b/test/benchmark.launch.xml @@ -0,0 +1,21 @@ + + + + + + + + + + + + + + + + + diff --git a/test/comparison.launch.xml b/test/comparison.launch.xml new file mode 100644 index 0000000..2634a24 --- /dev/null +++ b/test/comparison.launch.xml @@ -0,0 +1,25 @@ + + + + + + + + + + + + + + + + + + + + +