Implement socket diagnostics in bridge-node

.github/workflows/industrial_ci_action.yml

@@ -20,8 +20,8 @@ jobs:
       fail-fast: false
       matrix:
         ROS_DISTRO:
-          - humble
           - iron
+          - jazzy
         ROS_REPO:
           - main
     env:

CMakeLists.txt

@@ -17,6 +17,8 @@ endif()
 find_package(ament_cmake REQUIRED)
 find_package(ament_cmake_ros REQUIRED)
 find_package(can_msgs REQUIRED)
+find_package(diagnostic_msgs REQUIRED)
+find_package(diagnostic_updater REQUIRED)
 find_package(fmt REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(rclcpp_components REQUIRED)
@@ -30,6 +32,8 @@ target_include_directories(socketcan_bridge_component PUBLIC
   $<INSTALL_INTERFACE:include>)
 ament_target_dependencies(socketcan_bridge_component
   can_msgs
+  diagnostic_msgs
+  diagnostic_updater
   rclcpp
   rclcpp_components
 )

include/nobleo_socketcan_bridge/socketcan_bridge.hpp

@@ -21,6 +21,14 @@ class Clock;
 
 namespace nobleo_socketcan_bridge
 {
+enum class CanState
+{
+  OKAY,
+  WARN,
+  ERROR,
+  FATAL
+};
+
 class SocketCanBridge
 {
 public:
@@ -39,6 +47,8 @@ class SocketCanBridge
 
   void send(const can_msgs::msg::Frame & msg) const;
 
+  CanState getState() const { return state_; }
+
   void close();
 
 private:
@@ -54,6 +64,7 @@ class SocketCanBridge
   int socket_;
   CanCallback receive_callback_;
   std::jthread receive_thread_;
+  CanState state_;
 };
 
 can_frame from_msg(const can_msgs::msg::Frame & msg);

include/nobleo_socketcan_bridge/socketcan_bridge_node.hpp

@@ -4,6 +4,8 @@
 
 #pragma once
 
+#include <diagnostic_updater/diagnostic_updater.hpp>
+
 #include "nobleo_socketcan_bridge/socketcan_bridge.hpp"
 #include "rclcpp/node.hpp"
 
@@ -15,6 +17,9 @@ class SocketCanBridgeNode : public rclcpp::Node
   explicit SocketCanBridgeNode(const rclcpp::NodeOptions & options);
 
 private:
+  void produceDiagnostics(diagnostic_updater::DiagnosticStatusWrapper & status);
+
+  diagnostic_updater::Updater updater_;
   rclcpp::Publisher<can_msgs::msg::Frame>::SharedPtr can_pub;
   SocketCanBridge bridge;
   rclcpp::Subscription<can_msgs::msg::Frame>::SharedPtr can_sub;

package.xml

@@ -19,6 +19,8 @@ SPDX-License-Identifier: Apache-2.0
 
   <depend>ament_cmake_ros</depend>
   <depend>can_msgs</depend>
+  <depend>diagnostic_msgs</depend>
+  <depend>diagnostic_updater</depend>
   <depend>fmt</depend>
   <depend>rclcpp</depend>
   <depend>rclcpp_components</depend>

src/socketcan_bridge.cpp

@@ -7,6 +7,8 @@
 #include <fmt/core.h>
 #include <fmt/ostream.h>
 #include <linux/can.h>
+#include <linux/can/error.h>
+#include <linux/can/raw.h>
 #include <net/if.h>
 #include <sys/ioctl.h>
 #include <sys/socket.h>
@@ -94,6 +96,16 @@ void SocketCanBridge::connect()
     throw std::system_error(errno, std::generic_category());
   }
 
+  can_err_mask_t error_mask =
+    (CAN_ERR_BUSOFF      /* bus off */
+     | CAN_ERR_RESTARTED /* controller restarted */
+     | CAN_ERR_CRTL      /* controller problems / data[1]    */
+    );
+  if (setsockopt(socket_, SOL_CAN_RAW, CAN_RAW_ERR_FILTER, &error_mask, sizeof(error_mask)) < 0) {
+    RCLCPP_ERROR(logger_, "Error setting error mask");
+    throw std::system_error(errno, std::generic_category());
+  }
+
   ifreq ifr;
   strncpy(ifr.ifr_name, interface_.c_str(), sizeof(ifr.ifr_name));
   if (ioctl(socket_, SIOCGIFINDEX, &ifr) < 0) {
@@ -110,6 +122,7 @@ void SocketCanBridge::connect()
   }
 
   RCLCPP_INFO(logger_, "Connected to the CAN interface %s", interface_.c_str());
+  state_ = CanState::OKAY;
 }
 
 void SocketCanBridge::ensure_connection(std::stop_token stoken)
@@ -149,6 +162,7 @@ void SocketCanBridge::receive_loop(std::stop_token stoken)
       RCLCPP_ERROR(
         logger_, "Error reading from the socket: %s (%d), reconnecting in %.2f seconds ..",
         strerror(errno), static_cast<int>(errno), reconnect_timeout_);
+      state_ = CanState::FATAL;
       clock_->sleep_for(rclcpp::Duration::from_seconds(reconnect_timeout_));
       ensure_connection(stoken);
       continue;
@@ -159,6 +173,15 @@ void SocketCanBridge::receive_loop(std::stop_token stoken)
     }
 
     auto msg = to_msg(frame);
+    if (msg.is_error) {
+      // Based on data byte 1 select diagnostics level
+      if (frame.data[1] & CAN_ERR_CRTL_RX_WARNING || frame.data[1] & CAN_ERR_CRTL_TX_WARNING) {
+        state_ = CanState::WARN;
+      } else if (
+        frame.data[1] & CAN_ERR_CRTL_RX_PASSIVE || frame.data[1] & CAN_ERR_CRTL_TX_PASSIVE) {
+        state_ = CanState::ERROR;
+      }
+    }
     msg.header.stamp = clock_->now();
 
     RCLCPP_DEBUG_STREAM(logger_, "Received " << msg);
@@ -170,6 +193,7 @@ void SocketCanBridge::receive_loop(std::stop_token stoken)
 can_frame from_msg(const can_msgs::msg::Frame & msg)
 {
   canid_t id = msg.id;
+
   if (msg.is_rtr) id |= CAN_RTR_FLAG;
   if (msg.is_extended) id |= CAN_EFF_FLAG;
   if (msg.is_error) id |= CAN_ERR_FLAG;

src/socketcan_bridge_node.cpp

@@ -11,6 +11,7 @@ namespace nobleo_socketcan_bridge
 
 SocketCanBridgeNode::SocketCanBridgeNode(const rclcpp::NodeOptions & options)
 : rclcpp::Node("socketcan_bridge", options),
+  updater_(this),
   can_pub(this->create_publisher<can_msgs::msg::Frame>("~/rx", 100)),
   bridge(
     this->get_logger(), this->get_clock(), this->declare_parameter("interface", "can0"),
@@ -19,6 +20,30 @@ SocketCanBridgeNode::SocketCanBridgeNode(const rclcpp::NodeOptions & options)
   can_sub(this->create_subscription<can_msgs::msg::Frame>(
     "~/tx", 100, [this](can_msgs::msg::Frame::ConstSharedPtr msg) { bridge.send(*msg); }))
 {
+  updater_.setHardwareID("SocketCan");
+  updater_.add("SocketCan", [this](auto & stat) { this->produceDiagnostics(stat); });
+}
+
+void SocketCanBridgeNode::produceDiagnostics(diagnostic_updater::DiagnosticStatusWrapper & status)
+{
+  auto can_state = bridge.getState();
+  switch (can_state) {
+    case CanState::OKAY:
+      status.summary(diagnostic_msgs::msg::DiagnosticStatus::OK, "CAN interface is up");
+      break;
+    case CanState::WARN:
+      status.summary(
+        diagnostic_msgs::msg::DiagnosticStatus::WARN, "CAN interface is in warning state");
+      break;
+    case CanState::ERROR:
+      status.summary(
+        diagnostic_msgs::msg::DiagnosticStatus::ERROR, "CAN interface is in error state");
+      break;
+    case CanState::FATAL:
+      status.summary(
+        diagnostic_msgs::msg::DiagnosticStatus::ERROR, "Error connecting to CAN interface");
+      break;
+  }
 }
 
 }  // namespace nobleo_socketcan_bridge