From 7eb3db43726c2e1acda01f4d798e9e4292512a5e Mon Sep 17 00:00:00 2001 From: Kevin Hallenbeck Date: Wed, 22 Oct 2025 14:49:44 -0400 Subject: [PATCH 1/2] Add loopback option to sender --- ros2_socketcan/include/ros2_socketcan/socket_can_sender.hpp | 1 + .../include/ros2_socketcan/socket_can_sender_node.hpp | 1 + ros2_socketcan/launch/socket_can_bridge.launch.xml | 1 + ros2_socketcan/launch/socket_can_sender.launch.py | 2 ++ ros2_socketcan/src/socket_can_sender.cpp | 3 ++- ros2_socketcan/src/socket_can_sender_node.cpp | 6 +++++- 6 files changed, 12 insertions(+), 2 deletions(-) diff --git a/ros2_socketcan/include/ros2_socketcan/socket_can_sender.hpp b/ros2_socketcan/include/ros2_socketcan/socket_can_sender.hpp index cb2e8e8..c5c00fd 100644 --- a/ros2_socketcan/include/ros2_socketcan/socket_can_sender.hpp +++ b/ros2_socketcan/include/ros2_socketcan/socket_can_sender.hpp @@ -38,6 +38,7 @@ class SOCKETCAN_PUBLIC SocketCanSender explicit SocketCanSender( const std::string & interface = "can0", const bool enable_fd = false, + const bool enable_loopback = false, const CanId & default_id = CanId{}); /// Destructor ~SocketCanSender() noexcept; diff --git a/ros2_socketcan/include/ros2_socketcan/socket_can_sender_node.hpp b/ros2_socketcan/include/ros2_socketcan/socket_can_sender_node.hpp index 112a64e..6475ee7 100644 --- a/ros2_socketcan/include/ros2_socketcan/socket_can_sender_node.hpp +++ b/ros2_socketcan/include/ros2_socketcan/socket_can_sender_node.hpp @@ -76,6 +76,7 @@ class SOCKETCAN_PUBLIC SocketCanSenderNode final private: std::string interface_; bool enable_fd_; + bool enable_loopback_; rclcpp::Subscription::SharedPtr frames_sub_; rclcpp::Subscription::SharedPtr fd_frames_sub_; std::unique_ptr sender_; diff --git a/ros2_socketcan/launch/socket_can_bridge.launch.xml b/ros2_socketcan/launch/socket_can_bridge.launch.xml index 7cac86a..c62fe66 100644 --- a/ros2_socketcan/launch/socket_can_bridge.launch.xml +++ b/ros2_socketcan/launch/socket_can_bridge.launch.xml @@ -24,6 +24,7 @@ + diff --git a/ros2_socketcan/launch/socket_can_sender.launch.py b/ros2_socketcan/launch/socket_can_sender.launch.py index e72c228..84521d0 100644 --- a/ros2_socketcan/launch/socket_can_sender.launch.py +++ b/ros2_socketcan/launch/socket_can_sender.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'), 'timeout_sec': LaunchConfiguration('timeout_sec'), }], @@ -79,6 +80,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('timeout_sec', default_value='0.01'), DeclareLaunchArgument('auto_configure', default_value='true'), DeclareLaunchArgument('auto_activate', default_value='true'), diff --git a/ros2_socketcan/src/socket_can_sender.cpp b/ros2_socketcan/src/socket_can_sender.cpp index bf2cac3..3ddfbbe 100644 --- a/ros2_socketcan/src/socket_can_sender.cpp +++ b/ros2_socketcan/src/socket_can_sender.cpp @@ -36,9 +36,10 @@ namespace socketcan SocketCanSender::SocketCanSender( const std::string & interface, const bool enable_fd, + const bool enable_loopback, const CanId & default_id) : m_enable_fd(enable_fd), - m_file_descriptor{bind_can_socket(interface, m_enable_fd)}, + m_file_descriptor{bind_can_socket(interface, m_enable_fd, enable_loopback)}, m_default_id{default_id} { } diff --git a/ros2_socketcan/src/socket_can_sender_node.cpp b/ros2_socketcan/src/socket_can_sender_node.cpp index 9f7ab5b..8bce283 100644 --- a/ros2_socketcan/src/socket_can_sender_node.cpp +++ b/ros2_socketcan/src/socket_can_sender_node.cpp @@ -35,12 +35,16 @@ SocketCanSenderNode::SocketCanSenderNode(rclcpp::NodeOptions options) { interface_ = this->declare_parameter("interface", "can0"); enable_fd_ = this->declare_parameter("enable_can_fd", false); + enable_loopback_ = this->declare_parameter("enable_frame_loopback", false); double timeout_sec = this->declare_parameter("timeout_sec", 0.01); timeout_ns_ = std::chrono::duration_cast( std::chrono::duration(timeout_sec)); RCLCPP_INFO(this->get_logger(), "interface: %s", interface_.c_str()); 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(), "timeout(s): %f", timeout_sec); } @@ -49,7 +53,7 @@ LNI::CallbackReturn SocketCanSenderNode::on_configure(const lc::State & state) (void)state; try { - sender_ = std::make_unique(interface_, enable_fd_); + sender_ = std::make_unique(interface_, enable_fd_, enable_loopback_); } catch (const std::exception & ex) { RCLCPP_ERROR( this->get_logger(), "Error opening CAN sender: %s - %s", From 8e66a3225952850bc298d8c12a58883f9b65cf96 Mon Sep 17 00:00:00 2001 From: Kevin Hallenbeck Date: Wed, 22 Oct 2025 14:52:41 -0400 Subject: [PATCH 2/2] Remove loopback option from receiver Revert most of e39a814 --- .../include/ros2_socketcan/socket_can_receiver.hpp | 4 +--- .../include/ros2_socketcan/socket_can_receiver_node.hpp | 1 - ros2_socketcan/launch/socket_can_bridge.launch.xml | 1 - ros2_socketcan/launch/socket_can_receiver.launch.py | 2 -- ros2_socketcan/src/socket_can_receiver.cpp | 6 ++---- ros2_socketcan/src/socket_can_receiver_node.cpp | 6 +----- 6 files changed, 4 insertions(+), 16 deletions(-) diff --git a/ros2_socketcan/include/ros2_socketcan/socket_can_receiver.hpp b/ros2_socketcan/include/ros2_socketcan/socket_can_receiver.hpp index 379104c..0546170 100644 --- a/ros2_socketcan/include/ros2_socketcan/socket_can_receiver.hpp +++ b/ros2_socketcan/include/ros2_socketcan/socket_can_receiver.hpp @@ -39,9 +39,7 @@ class SOCKETCAN_PUBLIC SocketCanReceiver { public: /// Constructor - explicit SocketCanReceiver( - const std::string & interface = "can0", const bool enable_fd = false, - const bool enable_loopback = false); + explicit SocketCanReceiver(const std::string & interface = "can0", const bool enable_fd = 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 4a82793..7959eb7 100644 --- a/ros2_socketcan/include/ros2_socketcan/socket_can_receiver_node.hpp +++ b/ros2_socketcan/include/ros2_socketcan/socket_can_receiver_node.hpp @@ -81,7 +81,6 @@ 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 c62fe66..e4b00b3 100644 --- a/ros2_socketcan/launch/socket_can_bridge.launch.xml +++ b/ros2_socketcan/launch/socket_can_bridge.launch.xml @@ -14,7 +14,6 @@ - diff --git a/ros2_socketcan/launch/socket_can_receiver.launch.py b/ros2_socketcan/launch/socket_can_receiver.launch.py index 03ca3ce..2c411d0 100644 --- a/ros2_socketcan/launch/socket_can_receiver.launch.py +++ b/ros2_socketcan/launch/socket_can_receiver.launch.py @@ -37,7 +37,6 @@ 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'), @@ -82,7 +81,6 @@ 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_receiver.cpp b/ros2_socketcan/src/socket_can_receiver.cpp index 48e0c94..96aa2b2 100644 --- a/ros2_socketcan/src/socket_can_receiver.cpp +++ b/ros2_socketcan/src/socket_can_receiver.cpp @@ -37,10 +37,8 @@ namespace socketcan { //////////////////////////////////////////////////////////////////////////////// -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)}, +SocketCanReceiver::SocketCanReceiver(const std::string & interface, const bool enable_fd) +: m_file_descriptor{bind_can_socket(interface, enable_fd)}, 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 f67c29a..20c0e30 100644 --- a/ros2_socketcan/src/socket_can_receiver_node.cpp +++ b/ros2_socketcan/src/socket_can_receiver_node.cpp @@ -38,7 +38,6 @@ 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( @@ -47,9 +46,6 @@ 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); } @@ -58,7 +54,7 @@ LNI::CallbackReturn SocketCanReceiverNode::on_configure(const lc::State & state) (void)state; try { - receiver_ = std::make_unique(interface_, enable_fd_, enable_loopback_); + receiver_ = std::make_unique(interface_, enable_fd_); // apply CAN filters auto filters = get_parameter("filters").as_string(); receiver_->SetCanFilters(SocketCanReceiver::CanFilterList(filters));