diff --git a/include/ros2_socketcan_bridge/socketcan_bridge.hpp b/include/ros2_socketcan_bridge/socketcan_bridge.hpp index a7513c4..aea7ab2 100644 --- a/include/ros2_socketcan_bridge/socketcan_bridge.hpp +++ b/include/ros2_socketcan_bridge/socketcan_bridge.hpp @@ -30,19 +30,19 @@ class SocketCanBridge const rclcpp::Logger & logger, rclcpp::Clock::SharedPtr clock, const std::string & interface, double read_timeout, const CanCallback & receive_callback); - ~SocketCanBridge() { close(); } + ~SocketCanBridge(); SocketCanBridge(const SocketCanBridge &) = delete; SocketCanBridge(SocketCanBridge &&) noexcept = delete; SocketCanBridge & operator=(const SocketCanBridge & other) = delete; SocketCanBridge & operator=(SocketCanBridge && other) noexcept = delete; - void send(const can_msgs::msg::Frame & msg); + void send(const can_msgs::msg::Frame & msg) const; void close(); private: - void receive_loop(std::stop_token stoken); + void receive_loop(std::stop_token stoken) const; rclcpp::Logger logger_; rclcpp::Clock::SharedPtr clock_; diff --git a/src/socketcan_bridge.cpp b/src/socketcan_bridge.cpp index ba08ea3..177388c 100644 --- a/src/socketcan_bridge.cpp +++ b/src/socketcan_bridge.cpp @@ -63,7 +63,17 @@ SocketCanBridge::SocketCanBridge( receive_thread_ = std::jthread{std::bind(&SocketCanBridge::receive_loop, this, _1)}; } -void SocketCanBridge::send(const can_msgs::msg::Frame & msg) +SocketCanBridge::~SocketCanBridge() +{ + try { + close(); + } catch (const std::system_error & e) { + // closing the socket could fail, but we should not propagate exceptions out of the destructor + RCLCPP_ERROR(logger_, "%s", e.what()); + } +} + +void SocketCanBridge::send(const can_msgs::msg::Frame & msg) const { auto frame = from_msg(msg); RCLCPP_DEBUG_STREAM(logger_, "Sending " << msg); @@ -88,7 +98,7 @@ void SocketCanBridge::close() } } -void SocketCanBridge::receive_loop(std::stop_token stoken) +void SocketCanBridge::receive_loop(std::stop_token stoken) const { RCLCPP_INFO(logger_, "Receive loop started"); while (!stoken.stop_requested()) { @@ -99,7 +109,8 @@ void SocketCanBridge::receive_loop(std::stop_token stoken) throw std::system_error(errno, std::generic_category()); } if (nbytes != sizeof(frame)) { - throw std::runtime_error("Partial CAN frame received"); + RCLCPP_ERROR(logger_, "Incomplete CAN frame received, skipping"); + continue; } auto msg = to_msg(frame);