Skip to content

Commit

Permalink
Use send/receive instead of write/read
Browse files Browse the repository at this point in the history
  • Loading branch information
Rayman committed May 1, 2024
1 parent fd1d634 commit 3da7292
Show file tree
Hide file tree
Showing 3 changed files with 14 additions and 14 deletions.
6 changes: 3 additions & 3 deletions include/ros2_socketcan_bridge/socketcan_bridge.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -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
20 changes: 10 additions & 10 deletions src/socketcan_bridge.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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());
}
Expand All @@ -79,21 +79,21 @@ 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) {
throw std::system_error(errno, std::generic_category());
}
}

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));
Expand All @@ -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
2 changes: 1 addition & 1 deletion src/socketcan_bridge_node.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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<can_msgs::msg::Frame>(
"~/tx", 100, [this](can_msgs::msg::Frame::ConstSharedPtr msg) { bridge.write(*msg); }))
"~/tx", 100, [this](can_msgs::msg::Frame::ConstSharedPtr msg) { bridge.send(*msg); }))
{
}

Expand Down

0 comments on commit 3da7292

Please sign in to comment.