From ec4b1df1763c1df4edcc295b5bd956d3b739765e Mon Sep 17 00:00:00 2001 From: Ferry Schoenmakers Date: Mon, 12 Aug 2024 17:15:10 +0200 Subject: [PATCH 1/6] Implement socket diagnostics in bridge-node --- CMakeLists.txt | 2 ++ .../socketcan_bridge.hpp | 11 +++++++ .../socketcan_bridge_node.hpp | 5 ++++ package.xml | 2 ++ src/socketcan_bridge.cpp | 29 +++++++++++++++++++ src/socketcan_bridge_node.cpp | 25 ++++++++++++++++ 6 files changed, 74 insertions(+) diff --git a/CMakeLists.txt b/CMakeLists.txt index 117d92e..a54abbd 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -17,6 +17,7 @@ endif() find_package(ament_cmake REQUIRED) find_package(ament_cmake_ros REQUIRED) find_package(can_msgs REQUIRED) +find_package(diagnostic_updater REQUIRED) find_package(fmt REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_components REQUIRED) @@ -30,6 +31,7 @@ target_include_directories(socketcan_bridge_component PUBLIC $) ament_target_dependencies(socketcan_bridge_component can_msgs + diagnostic_updater rclcpp rclcpp_components ) diff --git a/include/nobleo_socketcan_bridge/socketcan_bridge.hpp b/include/nobleo_socketcan_bridge/socketcan_bridge.hpp index 4a3ef5d..a22758b 100644 --- a/include/nobleo_socketcan_bridge/socketcan_bridge.hpp +++ b/include/nobleo_socketcan_bridge/socketcan_bridge.hpp @@ -21,6 +21,14 @@ class Clock; namespace nobleo_socketcan_bridge { +enum class CAN_STATE : uint8_t +{ + OKAY, + WARN, + ERROR, + FATAL +}; + class SocketCanBridge { public: @@ -39,6 +47,8 @@ class SocketCanBridge void send(const can_msgs::msg::Frame & msg) const; + CAN_STATE getState() const { return state_; } + void close(); private: @@ -54,6 +64,7 @@ class SocketCanBridge int socket_; CanCallback receive_callback_; std::jthread receive_thread_; + CAN_STATE state_; }; can_frame from_msg(const can_msgs::msg::Frame & msg); diff --git a/include/nobleo_socketcan_bridge/socketcan_bridge_node.hpp b/include/nobleo_socketcan_bridge/socketcan_bridge_node.hpp index 643ff24..48c64eb 100644 --- a/include/nobleo_socketcan_bridge/socketcan_bridge_node.hpp +++ b/include/nobleo_socketcan_bridge/socketcan_bridge_node.hpp @@ -4,6 +4,8 @@ #pragma once +#include + #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::SharedPtr can_pub; SocketCanBridge bridge; rclcpp::Subscription::SharedPtr can_sub; diff --git a/package.xml b/package.xml index 7d5967b..2eb1e8f 100644 --- a/package.xml +++ b/package.xml @@ -19,6 +19,8 @@ SPDX-License-Identifier: Apache-2.0 ament_cmake_ros can_msgs + diagnostic_msgs + diagnostic_updater fmt rclcpp rclcpp_components diff --git a/src/socketcan_bridge.cpp b/src/socketcan_bridge.cpp index b196e05..d785c5a 100644 --- a/src/socketcan_bridge.cpp +++ b/src/socketcan_bridge.cpp @@ -7,11 +7,15 @@ #include #include #include +#include +#include #include #include #include #include +#include +#include #include "rclcpp/logging.hpp" @@ -94,6 +98,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 +124,7 @@ void SocketCanBridge::connect() } RCLCPP_INFO(logger_, "Connected to the CAN interface %s", interface_.c_str()); + state_ = CAN_STATE::OKAY; } void SocketCanBridge::ensure_connection(std::stop_token stoken) @@ -144,11 +159,13 @@ void SocketCanBridge::receive_loop(std::stop_token stoken) RCLCPP_DEBUG( logger_, "Error reading from the socket: %s (%d)", strerror(errno), static_cast(errno)); + state_ = CAN_STATE::FATAL; continue; } RCLCPP_ERROR( logger_, "Error reading from the socket: %s (%d), reconnecting in %.2f seconds ..", strerror(errno), static_cast(errno), reconnect_timeout_); + state_ = CAN_STATE::FATAL; clock_->sleep_for(rclcpp::Duration::from_seconds(reconnect_timeout_)); ensure_connection(stoken); continue; @@ -159,6 +176,17 @@ 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) { + std::cout << "Warning level reached" << std::endl; + state_ = CAN_STATE::WARN; + } else if ( + frame.data[1] & CAN_ERR_CRTL_RX_PASSIVE || frame.data[1] & CAN_ERR_CRTL_TX_PASSIVE) { + std::cout << "Error level reached" << std::endl; + state_ = CAN_STATE::ERROR; + } + } msg.header.stamp = clock_->now(); RCLCPP_DEBUG_STREAM(logger_, "Received " << msg); @@ -170,6 +198,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; diff --git a/src/socketcan_bridge_node.cpp b/src/socketcan_bridge_node.cpp index eed6b34..53c2e1f 100644 --- a/src/socketcan_bridge_node.cpp +++ b/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, 1.0), can_pub(this->create_publisher("~/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( "~/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 CAN_STATE::OKAY: + status.summary(diagnostic_msgs::msg::DiagnosticStatus::OK, "CAN interface is up"); + break; + case CAN_STATE::WARN: + status.summary( + diagnostic_msgs::msg::DiagnosticStatus::WARN, "CAN interface is in warning state"); + break; + case CAN_STATE::ERROR: + status.summary( + diagnostic_msgs::msg::DiagnosticStatus::ERROR, "CAN interface is in error state"); + break; + case CAN_STATE::FATAL: + status.summary( + diagnostic_msgs::msg::DiagnosticStatus::ERROR, "Error connecting to CAN interface"); + break; + } } } // namespace nobleo_socketcan_bridge From 513f4ad68d53cf0c29dd99433f0d9bbd5a91f3a7 Mon Sep 17 00:00:00 2001 From: Ferry Schoenmakers Date: Mon, 12 Aug 2024 17:17:03 +0200 Subject: [PATCH 2/6] Cleanup prints... --- src/socketcan_bridge.cpp | 4 ---- 1 file changed, 4 deletions(-) diff --git a/src/socketcan_bridge.cpp b/src/socketcan_bridge.cpp index d785c5a..e5cdfab 100644 --- a/src/socketcan_bridge.cpp +++ b/src/socketcan_bridge.cpp @@ -14,8 +14,6 @@ #include #include -#include -#include #include "rclcpp/logging.hpp" @@ -179,11 +177,9 @@ void SocketCanBridge::receive_loop(std::stop_token stoken) 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) { - std::cout << "Warning level reached" << std::endl; state_ = CAN_STATE::WARN; } else if ( frame.data[1] & CAN_ERR_CRTL_RX_PASSIVE || frame.data[1] & CAN_ERR_CRTL_TX_PASSIVE) { - std::cout << "Error level reached" << std::endl; state_ = CAN_STATE::ERROR; } } From e8467cd37a53c0bcba2a71eb83ba3116961d32e8 Mon Sep 17 00:00:00 2001 From: Ferry Schoenmakers Date: Tue, 13 Aug 2024 08:55:24 +0200 Subject: [PATCH 3/6] Ditch Humble and test Jazzy instead --- .github/workflows/industrial_ci_action.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/industrial_ci_action.yml b/.github/workflows/industrial_ci_action.yml index 2bafeab..a581198 100644 --- a/.github/workflows/industrial_ci_action.yml +++ b/.github/workflows/industrial_ci_action.yml @@ -20,8 +20,8 @@ jobs: fail-fast: false matrix: ROS_DISTRO: - - humble - iron + - jazzy ROS_REPO: - main env: From 5eef99437d586523f9cb9d67da8c69ec78da3112 Mon Sep 17 00:00:00 2001 From: Ferry Schoenmakers Date: Tue, 13 Aug 2024 08:55:38 +0200 Subject: [PATCH 4/6] Review comments --- include/nobleo_socketcan_bridge/socketcan_bridge.hpp | 6 +++--- package.xml | 1 - src/socketcan_bridge.cpp | 10 +++++----- src/socketcan_bridge_node.cpp | 10 +++++----- 4 files changed, 13 insertions(+), 14 deletions(-) diff --git a/include/nobleo_socketcan_bridge/socketcan_bridge.hpp b/include/nobleo_socketcan_bridge/socketcan_bridge.hpp index a22758b..1b92af9 100644 --- a/include/nobleo_socketcan_bridge/socketcan_bridge.hpp +++ b/include/nobleo_socketcan_bridge/socketcan_bridge.hpp @@ -21,7 +21,7 @@ class Clock; namespace nobleo_socketcan_bridge { -enum class CAN_STATE : uint8_t +enum class CanState { OKAY, WARN, @@ -47,7 +47,7 @@ class SocketCanBridge void send(const can_msgs::msg::Frame & msg) const; - CAN_STATE getState() const { return state_; } + CanState getState() const { return state_; } void close(); @@ -64,7 +64,7 @@ class SocketCanBridge int socket_; CanCallback receive_callback_; std::jthread receive_thread_; - CAN_STATE state_; + CanState state_; }; can_frame from_msg(const can_msgs::msg::Frame & msg); diff --git a/package.xml b/package.xml index 2eb1e8f..b4ecfa6 100644 --- a/package.xml +++ b/package.xml @@ -19,7 +19,6 @@ SPDX-License-Identifier: Apache-2.0 ament_cmake_ros can_msgs - diagnostic_msgs diagnostic_updater fmt rclcpp diff --git a/src/socketcan_bridge.cpp b/src/socketcan_bridge.cpp index e5cdfab..33619ad 100644 --- a/src/socketcan_bridge.cpp +++ b/src/socketcan_bridge.cpp @@ -122,7 +122,7 @@ void SocketCanBridge::connect() } RCLCPP_INFO(logger_, "Connected to the CAN interface %s", interface_.c_str()); - state_ = CAN_STATE::OKAY; + state_ = CanState::OKAY; } void SocketCanBridge::ensure_connection(std::stop_token stoken) @@ -157,13 +157,13 @@ void SocketCanBridge::receive_loop(std::stop_token stoken) RCLCPP_DEBUG( logger_, "Error reading from the socket: %s (%d)", strerror(errno), static_cast(errno)); - state_ = CAN_STATE::FATAL; + state_ = CanState::FATAL; continue; } RCLCPP_ERROR( logger_, "Error reading from the socket: %s (%d), reconnecting in %.2f seconds ..", strerror(errno), static_cast(errno), reconnect_timeout_); - state_ = CAN_STATE::FATAL; + state_ = CanState::FATAL; clock_->sleep_for(rclcpp::Duration::from_seconds(reconnect_timeout_)); ensure_connection(stoken); continue; @@ -177,10 +177,10 @@ void SocketCanBridge::receive_loop(std::stop_token stoken) 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_ = CAN_STATE::WARN; + state_ = CanState::WARN; } else if ( frame.data[1] & CAN_ERR_CRTL_RX_PASSIVE || frame.data[1] & CAN_ERR_CRTL_TX_PASSIVE) { - state_ = CAN_STATE::ERROR; + state_ = CanState::ERROR; } } msg.header.stamp = clock_->now(); diff --git a/src/socketcan_bridge_node.cpp b/src/socketcan_bridge_node.cpp index 53c2e1f..8937344 100644 --- a/src/socketcan_bridge_node.cpp +++ b/src/socketcan_bridge_node.cpp @@ -11,7 +11,7 @@ namespace nobleo_socketcan_bridge SocketCanBridgeNode::SocketCanBridgeNode(const rclcpp::NodeOptions & options) : rclcpp::Node("socketcan_bridge", options), - updater_(this, 1.0), + updater_(this), can_pub(this->create_publisher("~/rx", 100)), bridge( this->get_logger(), this->get_clock(), this->declare_parameter("interface", "can0"), @@ -28,18 +28,18 @@ void SocketCanBridgeNode::produceDiagnostics(diagnostic_updater::DiagnosticStatu { auto can_state = bridge.getState(); switch (can_state) { - case CAN_STATE::OKAY: + case CanState::OKAY: status.summary(diagnostic_msgs::msg::DiagnosticStatus::OK, "CAN interface is up"); break; - case CAN_STATE::WARN: + case CanState::WARN: status.summary( diagnostic_msgs::msg::DiagnosticStatus::WARN, "CAN interface is in warning state"); break; - case CAN_STATE::ERROR: + case CanState::ERROR: status.summary( diagnostic_msgs::msg::DiagnosticStatus::ERROR, "CAN interface is in error state"); break; - case CAN_STATE::FATAL: + case CanState::FATAL: status.summary( diagnostic_msgs::msg::DiagnosticStatus::ERROR, "Error connecting to CAN interface"); break; From df2786a6f8cc01c75fa07243d582d14ee623dacf Mon Sep 17 00:00:00 2001 From: Ferry Schoenmakers Date: Tue, 13 Aug 2024 09:27:58 +0200 Subject: [PATCH 5/6] Don't error on no incoming messages --- src/socketcan_bridge.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/src/socketcan_bridge.cpp b/src/socketcan_bridge.cpp index 33619ad..85c039d 100644 --- a/src/socketcan_bridge.cpp +++ b/src/socketcan_bridge.cpp @@ -157,7 +157,6 @@ void SocketCanBridge::receive_loop(std::stop_token stoken) RCLCPP_DEBUG( logger_, "Error reading from the socket: %s (%d)", strerror(errno), static_cast(errno)); - state_ = CanState::FATAL; continue; } RCLCPP_ERROR( From c01168faa6610e9d29d7a29ad43355f390123dd6 Mon Sep 17 00:00:00 2001 From: Ferry Schoenmakers Date: Tue, 13 Aug 2024 09:29:25 +0200 Subject: [PATCH 6/6] Add diagnostic_msgs dependencies --- CMakeLists.txt | 2 ++ package.xml | 1 + 2 files changed, 3 insertions(+) diff --git a/CMakeLists.txt b/CMakeLists.txt index a54abbd..fa4bdd0 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -17,6 +17,7 @@ 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) @@ -31,6 +32,7 @@ target_include_directories(socketcan_bridge_component PUBLIC $) ament_target_dependencies(socketcan_bridge_component can_msgs + diagnostic_msgs diagnostic_updater rclcpp rclcpp_components diff --git a/package.xml b/package.xml index b4ecfa6..2eb1e8f 100644 --- a/package.xml +++ b/package.xml @@ -19,6 +19,7 @@ SPDX-License-Identifier: Apache-2.0 ament_cmake_ros can_msgs + diagnostic_msgs diagnostic_updater fmt rclcpp