Add loopback parameter

Signed-off-by: Joshua Whitley <[email protected]>
autowarefoundation · Dec 4, 2024 · ef3e8ac · ef3e8ac
1 parent fb45862
commit ef3e8ac
Show file tree

Hide file tree

Showing 6 changed files with 17 additions and 4 deletions.
diff --git 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
@@ -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
@@ -4,6 +4,7 @@
   <arg name="receiver_interval_sec" default="0.01" />
   <arg name="sender_timeout_sec" default="0.01" />
   <arg name="enable_can_fd" default="false" />
+  <arg name="enable_frame_loopback" default="false" />
   <arg name="from_can_bus_topic" default="from_can_bus" />
   <arg name="to_can_bus_topic" default="to_can_bus" />
   <arg name="use_bus_time" default="false" />
@@ -12,6 +13,7 @@
     <arg name="interface" value="$(var interface)" />
     <arg name="interval_sec" value="$(var receiver_interval_sec)" />
     <arg name="enable_can_fd" value="$(var enable_can_fd)" />
+    <arg name="enable_frame_loopback" value="$(var enable_frame_loopback)" />
     <arg name="from_can_bus_topic" value="$(var from_can_bus_topic)" />
     <arg name="use_bus_time" value="$(var use_bus_time)" />
   </include>

diff --git 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_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
@@ -38,6 +38,7 @@ SocketCanReceiverNode::SocketCanReceiverNode(rclcpp::NodeOptions options)
   interface_ = this->declare_parameter("interface", "can0");
   use_bus_time_ = this->declare_parameter<bool>("use_bus_time", false);
   enable_fd_ = this->declare_parameter<bool>("enable_can_fd", false);
+  enable_loopback_ = this->declare_parameter<bool>("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<std::chrono::nanoseconds>(
@@ -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<SocketCanReceiver>(interface_, enable_fd_);
+    receiver_ = std::make_unique<SocketCanReceiver>(interface_, enable_fd_, enable_loopback_);
     // apply CAN filters
     auto filters = get_parameter("filters").as_string();
     receiver_->SetCanFilters(SocketCanReceiver::CanFilterList(filters));