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));