diff --git a/ros2_socketcan/include/ros2_socketcan/socket_can_common.hpp b/ros2_socketcan/include/ros2_socketcan/socket_can_common.hpp index 65a7d9c..f337c4d 100644 --- a/ros2_socketcan/include/ros2_socketcan/socket_can_common.hpp +++ b/ros2_socketcan/include/ros2_socketcan/socket_can_common.hpp @@ -34,10 +34,14 @@ namespace socketcan /// Bind a non-blocking CAN_RAW socket to the given interface /// \param[in] interface The name of the interface to bind, must be smaller than IFNAMSIZ /// \param[in] enable_fd Whether this socket uses CAN FD or not +/// \param[in] enable_loopback Whether this socket will receive sent messages from other processes +/// connected to the same socket on the same system /// \return The file descriptor bound to the given interface /// \throw std::runtime_error If one of socket(), fnctl(), ioctl(), bind() failed /// \throw std::domain_error If the provided interface name is too long -int32_t bind_can_socket(const std::string & interface, bool enable_fd); +int32_t bind_can_socket( + const std::string & interface, bool enable_fd, + bool enable_loopback = false); /// Set SocketCAN filters /// \param[in] fd File descriptor of the socket diff --git a/ros2_socketcan/include/ros2_socketcan/socket_can_receiver.hpp b/ros2_socketcan/include/ros2_socketcan/socket_can_receiver.hpp index 0546170..379104c 100644 --- a/ros2_socketcan/include/ros2_socketcan/socket_can_receiver.hpp +++ b/ros2_socketcan/include/ros2_socketcan/socket_can_receiver.hpp @@ -39,7 +39,9 @@ class SOCKETCAN_PUBLIC SocketCanReceiver { public: /// Constructor - explicit SocketCanReceiver(const std::string & interface = "can0", const bool enable_fd = false); + explicit SocketCanReceiver( + const std::string & interface = "can0", const bool enable_fd = false, + const bool enable_loopback = false); /// Destructor ~SocketCanReceiver() noexcept; diff --git a/ros2_socketcan/include/ros2_socketcan/socket_can_receiver_node.hpp b/ros2_socketcan/include/ros2_socketcan/socket_can_receiver_node.hpp index 7959eb7..4a82793 100644 --- a/ros2_socketcan/include/ros2_socketcan/socket_can_receiver_node.hpp +++ b/ros2_socketcan/include/ros2_socketcan/socket_can_receiver_node.hpp @@ -81,6 +81,7 @@ class SOCKETCAN_PUBLIC SocketCanReceiverNode final std::chrono::nanoseconds interval_ns_; bool enable_fd_; bool use_bus_time_; + bool enable_loopback_; }; } // namespace socketcan } // namespace drivers diff --git a/ros2_socketcan/launch/socket_can_bridge.launch.xml b/ros2_socketcan/launch/socket_can_bridge.launch.xml index cbfd729..9c0b543 100644 --- a/ros2_socketcan/launch/socket_can_bridge.launch.xml +++ b/ros2_socketcan/launch/socket_can_bridge.launch.xml @@ -4,6 +4,7 @@ + @@ -12,6 +13,7 @@ + diff --git a/ros2_socketcan/launch/socket_can_receiver.launch.py b/ros2_socketcan/launch/socket_can_receiver.launch.py index 080541c..3624ba1 100644 --- a/ros2_socketcan/launch/socket_can_receiver.launch.py +++ b/ros2_socketcan/launch/socket_can_receiver.launch.py @@ -37,6 +37,7 @@ def generate_launch_description(): parameters=[{ 'interface': LaunchConfiguration('interface'), 'enable_can_fd': LaunchConfiguration('enable_can_fd'), + 'enable_frame_loopback': LaunchConfiguration('enable_frame_loopback'), 'interval_sec': LaunchConfiguration('interval_sec'), 'filters': LaunchConfiguration('filters'), @@ -80,6 +81,7 @@ def generate_launch_description(): return LaunchDescription([ DeclareLaunchArgument('interface', default_value='can0'), DeclareLaunchArgument('enable_can_fd', default_value='false'), + DeclareLaunchArgument('enable_frame_loopback', default_value='false'), DeclareLaunchArgument('interval_sec', default_value='0.01'), DeclareLaunchArgument('use_bus_time', default_value='false'), DeclareLaunchArgument('filters', default_value='0:0', diff --git a/ros2_socketcan/src/socket_can_common.cpp b/ros2_socketcan/src/socket_can_common.cpp index 3d1b4e3..ae97c89 100644 --- a/ros2_socketcan/src/socket_can_common.cpp +++ b/ros2_socketcan/src/socket_can_common.cpp @@ -37,7 +37,7 @@ namespace socketcan { //////////////////////////////////////////////////////////////////////////////// -int32_t bind_can_socket(const std::string & interface, bool enable_fd) +int32_t bind_can_socket(const std::string & interface, bool enable_fd, bool enable_loopback) { if (interface.length() >= static_cast(IFNAMSIZ)) { throw std::domain_error{"CAN interface name too long"}; @@ -73,6 +73,14 @@ int32_t bind_can_socket(const std::string & interface, bool enable_fd) } //lint -restore NOLINT + // Enable local message loopback + const int32_t loopback = enable_loopback ? 1 : 0; + if (0 != + setsockopt(file_descriptor, SOL_CAN_RAW, CAN_RAW_LOOPBACK, &loopback, sizeof(loopback))) + { + throw std::runtime_error{"Failed to set local loopback option"}; + } + // Enable CAN FD support const int32_t enable_canfd = enable_fd ? 1 : 0; if (0 != @@ -80,7 +88,7 @@ int32_t bind_can_socket(const std::string & interface, bool enable_fd) file_descriptor, SOL_CAN_RAW, CAN_RAW_FD_FRAMES, &enable_canfd, sizeof(enable_canfd))) { - throw std::runtime_error{"Failed to enable CAN FD support"}; + throw std::runtime_error{"Failed to set CAN FD support option"}; } return file_descriptor; diff --git a/ros2_socketcan/src/socket_can_receiver.cpp b/ros2_socketcan/src/socket_can_receiver.cpp index 96aa2b2..48e0c94 100644 --- a/ros2_socketcan/src/socket_can_receiver.cpp +++ b/ros2_socketcan/src/socket_can_receiver.cpp @@ -37,8 +37,10 @@ namespace socketcan { //////////////////////////////////////////////////////////////////////////////// -SocketCanReceiver::SocketCanReceiver(const std::string & interface, const bool enable_fd) -: m_file_descriptor{bind_can_socket(interface, enable_fd)}, +SocketCanReceiver::SocketCanReceiver( + const std::string & interface, const bool enable_fd, + const bool enable_loopback) +: m_file_descriptor{bind_can_socket(interface, enable_fd, enable_loopback)}, m_enable_fd(enable_fd) { } diff --git a/ros2_socketcan/src/socket_can_receiver_node.cpp b/ros2_socketcan/src/socket_can_receiver_node.cpp index 20c0e30..f67c29a 100644 --- a/ros2_socketcan/src/socket_can_receiver_node.cpp +++ b/ros2_socketcan/src/socket_can_receiver_node.cpp @@ -38,6 +38,7 @@ SocketCanReceiverNode::SocketCanReceiverNode(rclcpp::NodeOptions options) interface_ = this->declare_parameter("interface", "can0"); use_bus_time_ = this->declare_parameter("use_bus_time", false); enable_fd_ = this->declare_parameter("enable_can_fd", false); + enable_loopback_ = this->declare_parameter("enable_frame_loopback", false); double interval_sec = this->declare_parameter("interval_sec", 0.01); this->declare_parameter("filters", "0:0"); interval_ns_ = std::chrono::duration_cast( @@ -46,6 +47,9 @@ SocketCanReceiverNode::SocketCanReceiverNode(rclcpp::NodeOptions options) RCLCPP_INFO(this->get_logger(), "interface: %s", interface_.c_str()); RCLCPP_INFO(this->get_logger(), "use bus time: %d", use_bus_time_); RCLCPP_INFO(this->get_logger(), "can fd enabled: %s", enable_fd_ ? "true" : "false"); + RCLCPP_INFO( + this->get_logger(), "frame loopback enabled: %s", + enable_loopback_ ? "true" : "false"); RCLCPP_INFO(this->get_logger(), "interval(s): %f", interval_sec); } @@ -54,7 +58,7 @@ LNI::CallbackReturn SocketCanReceiverNode::on_configure(const lc::State & state) (void)state; try { - receiver_ = std::make_unique(interface_, enable_fd_); + receiver_ = std::make_unique(interface_, enable_fd_, enable_loopback_); // apply CAN filters auto filters = get_parameter("filters").as_string(); receiver_->SetCanFilters(SocketCanReceiver::CanFilterList(filters));