From 3da72923b8c51f82565fd2920d8b1a11c0ba00fa Mon Sep 17 00:00:00 2001 From: Ramon Wijnands Date: Wed, 1 May 2024 09:56:24 +0200 Subject: [PATCH] Use send/receive instead of write/read --- .../socketcan_bridge.hpp | 6 +++--- src/socketcan_bridge.cpp | 20 +++++++++---------- src/socketcan_bridge_node.cpp | 2 +- 3 files changed, 14 insertions(+), 14 deletions(-) diff --git a/include/ros2_socketcan_bridge/socketcan_bridge.hpp b/include/ros2_socketcan_bridge/socketcan_bridge.hpp index ef3ae27..91b5972 100644 --- a/include/ros2_socketcan_bridge/socketcan_bridge.hpp +++ b/include/ros2_socketcan_bridge/socketcan_bridge.hpp @@ -35,18 +35,18 @@ class SocketCanBridge SocketCanBridge & operator=(const SocketCanBridge & other) = delete; SocketCanBridge & operator=(SocketCanBridge && other) noexcept = delete; - void __attribute__((noinline)) write(const can_msgs::msg::Frame & msg); + void send(const can_msgs::msg::Frame & msg); void close(); private: - void read_loop(std::stop_token stoken); + void receive_loop(std::stop_token stoken); rclcpp::Logger logger_; rclcpp::Clock::SharedPtr clock_; int socket_; CanCallback receive_callback_; - std::jthread read_thread_; + std::jthread receive_thread_; }; } // namespace ros2_socketcan_bridge diff --git a/src/socketcan_bridge.cpp b/src/socketcan_bridge.cpp index edaf36a..1daa923 100644 --- a/src/socketcan_bridge.cpp +++ b/src/socketcan_bridge.cpp @@ -60,17 +60,17 @@ SocketCanBridge::SocketCanBridge( throw std::system_error(errno, std::generic_category()); } - read_thread_ = std::jthread{std::bind(&SocketCanBridge::read_loop, this, _1)}; + receive_thread_ = std::jthread{std::bind(&SocketCanBridge::receive_loop, this, _1)}; } -void SocketCanBridge::write(const can_msgs::msg::Frame & msg) +void SocketCanBridge::send(const can_msgs::msg::Frame & msg) { can_frame frame; frame.can_id = msg.id; frame.len = msg.dlc; std::copy(msg.data.begin(), msg.data.end(), frame.data); RCLCPP_DEBUG_STREAM(logger_, "Sending " << msg); - auto n = ::write(socket_, &frame, sizeof(frame)); + auto n = write(socket_, &frame, sizeof(frame)); if (n < 0) { throw std::system_error(errno, std::generic_category()); } @@ -79,11 +79,11 @@ void SocketCanBridge::write(const can_msgs::msg::Frame & msg) void SocketCanBridge::close() { - RCLCPP_INFO(logger_, "Stopping the read thread"); - read_thread_.request_stop(); + RCLCPP_INFO(logger_, "Stopping the receive thread"); + receive_thread_.request_stop(); - RCLCPP_INFO(logger_, "Waiting for the read thread to stop"); - read_thread_.join(); + RCLCPP_INFO(logger_, "Waiting for the receive thread to stop"); + receive_thread_.join(); RCLCPP_INFO(logger_, "Closing the socket"); if (::close(socket_) < 0) { @@ -91,9 +91,9 @@ void SocketCanBridge::close() } } -void SocketCanBridge::read_loop(std::stop_token stoken) +void SocketCanBridge::receive_loop(std::stop_token stoken) { - RCLCPP_INFO(logger_, "Read loop started"); + RCLCPP_INFO(logger_, "Receive loop started"); while (!stoken.stop_requested()) { can_frame frame; auto nbytes = read(socket_, &frame, sizeof(can_frame)); @@ -114,7 +114,7 @@ void SocketCanBridge::read_loop(std::stop_token stoken) RCLCPP_DEBUG_STREAM(logger_, "Received " << msg); receive_callback_(msg); } - RCLCPP_INFO(logger_, "Read loop stopped"); + RCLCPP_INFO(logger_, "Receive loop stopped"); } } // namespace ros2_socketcan_bridge diff --git a/src/socketcan_bridge_node.cpp b/src/socketcan_bridge_node.cpp index 93e392a..50c4a56 100644 --- a/src/socketcan_bridge_node.cpp +++ b/src/socketcan_bridge_node.cpp @@ -17,7 +17,7 @@ SocketCanBridgeNode::SocketCanBridgeNode(const rclcpp::NodeOptions & options) this->declare_parameter("read_timeout", 1.0), [this](const can_msgs::msg::Frame & msg) { can_pub->publish(msg); }), can_sub(this->create_subscription( - "~/tx", 100, [this](can_msgs::msg::Frame::ConstSharedPtr msg) { bridge.write(*msg); })) + "~/tx", 100, [this](can_msgs::msg::Frame::ConstSharedPtr msg) { bridge.send(*msg); })) { }