From 20b2d27725e9ef3e39254409973ebc84b5b2c50a Mon Sep 17 00:00:00 2001 From: naga Date: Fri, 5 Apr 2024 14:16:57 +0900 Subject: [PATCH 1/2] transmit upgrade --- include/slcan_cpp/slcan.hpp | 5 ++++- src/slcan_impl.cpp | 29 ++++++++++++----------------- 2 files changed, 16 insertions(+), 18 deletions(-) diff --git a/include/slcan_cpp/slcan.hpp b/include/slcan_cpp/slcan.hpp index 76e7469..0e0af8c 100644 --- a/include/slcan_cpp/slcan.hpp +++ b/include/slcan_cpp/slcan.hpp @@ -32,8 +32,9 @@ class slcan_node : public rclcpp::Node rclcpp::Publisher::SharedPtr m_pub_data; rclcpp::Publisher::SharedPtr m_uart_fail_pub; rclcpp::TimerBase::SharedPtr m_recv_timer; + rclcpp::TimerBase::SharedPtr m_send_timer; - std::thread m_write; + std::queue m_message_queue; bool open_serial_port(); @@ -52,6 +53,8 @@ class slcan_node : public rclcpp::Node can_msgs::msg::CanMsg decode_data(const std::string &data); void sub_callback(can_msgs::msg::CanMsg::SharedPtr msg); + + void transmit_timer_callback(); public: slcan_node(); diff --git a/src/slcan_impl.cpp b/src/slcan_impl.cpp index 06381df..5241ae2 100644 --- a/src/slcan_impl.cpp +++ b/src/slcan_impl.cpp @@ -16,6 +16,7 @@ slcan_node::slcan_node() : rclcpp::Node("slcan_node") m_recv_timer = this->create_wall_timer( 1ms, std::bind(&slcan_node::recv_timer_callback, this)); m_uart_fail_pub = this->create_publisher("uart_fail", 10); + m_send_timer = this->create_wall_timer(1ms, std::bind(&slcan_node::transmit_timer_callback, this)); auto is_open = this->open_serial_port(); @@ -126,7 +127,7 @@ void slcan_node::send(const std::string& data) { tcdrain(m_fd); - int ret = ::write(m_fd, data.c_str(), data.size()); + ::write(m_fd, data.c_str(), data.size()); #if SHOW_SENDING_MSG RCLCPP_INFO(this->get_logger(), "send: %s", data.c_str()); #endif // SHOW_SENDING @@ -281,24 +282,18 @@ can_msgs::msg::CanMsg slcan_node::decode_data(const std::string &data) void slcan_node::sub_callback(const can_msgs::msg::CanMsg::SharedPtr msg) { - auto data = encode_data(*msg); - - send(data); - - auto data_msg = decode_data(data); + m_message_queue.push(msg); +} - if(msg->data.size() == 4) +void slcan_node::transmit_timer_callback() +{ + while(m_message_queue.size() > 0) { - uint32_t data = 0; - for(uint32_t i = 0; i < msg->data.size(); i++) - { - data |= data_msg.data[i] << (8*i); - } - auto f = std::bit_cast(data); + auto msg = m_message_queue.front(); + m_message_queue.pop(); - #if SHOW_SUBSCRIPTION - RCLCPP_INFO(this->get_logger(), "msg: {id: %x ,data: %f}", data_msg.id, f); - #endif // SHOW_SUBSCRIPTION + send(encode_data(*msg)); + + RCLCPP_INFO(this->get_logger(), "send: %s", encode_data(*msg).c_str()); } - } From 624ebc6580ee269efbdf085a525a0e8f50ff0faa Mon Sep 17 00:00:00 2001 From: naga Date: Sat, 4 May 2024 21:16:48 +0900 Subject: [PATCH 2/2] update termios config --- src/slcan_impl.cpp | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/src/slcan_impl.cpp b/src/slcan_impl.cpp index 5241ae2..336f24d 100644 --- a/src/slcan_impl.cpp +++ b/src/slcan_impl.cpp @@ -68,7 +68,7 @@ bool slcan_node::open_serial_port() } // シリアルポートをオープン - m_fd = open(device.c_str(), O_RDWR | O_NOCTTY | O_SYNC); + m_fd = open(device.c_str(), O_RDWR | O_NOCTTY); if (m_fd < 0) { RCLCPP_ERROR(this->get_logger(), "can't open %s", device.c_str()); @@ -95,14 +95,14 @@ bool slcan_node::open_serial_port() return false; } + bzero(&term, sizeof(term)); + cfsetospeed(&term, UART_BAUDRATE); cfsetispeed(&term, UART_BAUDRATE); - term.c_cflag &= ~CSIZE; - term.c_cflag |= CS8; - term.c_cflag &= ~CRTSCTS; - term.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG); - term.c_iflag &= ~(IXON | IXOFF | IXANY | ICRNL | INLCR | IGNCR); - term.c_oflag &= ~OPOST; + term.c_cflag |= UART_BAUDRATE | CS8 | CREAD; + term.c_lflag |= ICANON; + term.c_iflag = ICRNL | IGNPAR; + term.c_oflag = 0; term.c_cc[VMIN] = 0; term.c_cc[VTIME]= 0; @@ -145,7 +145,7 @@ void slcan_node::recv_timer_callback() return ; } - constexpr size_t buffer_size = 19; + constexpr size_t buffer_size = 256; char buff[buffer_size]; int n = read(m_fd, buff, buffer_size);