From 3ce6468112df62a2c1cd9224a1e19e34d8be1e37 Mon Sep 17 00:00:00 2001 From: Andrea Scipione Date: Sat, 4 Nov 2023 16:20:59 +0000 Subject: [PATCH] switched to async_read for bad performance with multiple vesc --- vesc_driver/src/vesc_interface.cpp | 124 ++++++++++++++--------------- 1 file changed, 59 insertions(+), 65 deletions(-) diff --git a/vesc_driver/src/vesc_interface.cpp b/vesc_driver/src/vesc_interface.cpp index 9557431..20d7155 100644 --- a/vesc_driver/src/vesc_interface.cpp +++ b/vesc_driver/src/vesc_interface.cpp @@ -54,7 +54,7 @@ class VescInterface::Impl : owned_ctx{new IoContext(2)}, serial_driver_{new drivers::serial_driver::SerialDriver(*owned_ctx)} {} - void packet_creation_thread(); + void receive_callback(const std::vector & buffer, const size_t & bytes_transferred); void on_configure(); void connect(const std::string & port); @@ -78,69 +78,66 @@ class VescInterface::Impl std::vector buffer_; }; -void VescInterface::Impl::packet_creation_thread() +void VescInterface::Impl::receive_callback( + const std::vector & temp_buffer, + const size_t & bytes_transferred +) { - static auto temp_buffer = Buffer(2048, 0); - while (packet_thread_run_) { - const auto bytes_read = serial_driver_->port()->receive(temp_buffer); - buffer_.reserve(buffer_.size() + temp_buffer.size()); - buffer_.insert(buffer_.end(), temp_buffer.begin(), temp_buffer.begin() + bytes_read); - int bytes_needed = VescFrame::VESC_MIN_FRAME_SIZE; - if (!buffer_.empty()) { - // search buffer for valid packet(s) - auto iter = buffer_.begin(); - auto iter_begin = buffer_.begin(); - while (iter != buffer_.end()) { - // check if valid start-of-frame character - if (VescFrame::VESC_SOF_VAL_SMALL_FRAME == *iter || - VescFrame::VESC_SOF_VAL_LARGE_FRAME == *iter) - { - // good start, now attempt to create packet - std::string error; - VescPacketConstPtr packet = - VescPacketFactory::createPacket(iter, buffer_.end(), &bytes_needed, &error); - if (packet) { - // good packet, check if we skipped any data - if (std::distance(iter_begin, iter) > 0) { - std::ostringstream ss; - ss << "Out-of-sync with VESC, unknown data leading valid frame. Discarding " << - std::distance(iter_begin, iter) << " bytes."; - error_handler_(ss.str()); - } - // call packet handler - packet_handler_(packet); - // update state - iter = iter + packet->frame().size(); - iter_begin = iter; - // continue to look for another frame in buffer - continue; - } else if (bytes_needed > 0) { - // need more data, break out of while loop - break; // for (iter_sof... - } else { - // else, this was not a packet, move on to next byte - error_handler_(error); + buffer_.reserve(buffer_.size() + bytes_transferred); + buffer_.insert(buffer_.end(), temp_buffer.begin(), temp_buffer.begin() + bytes_transferred); + int bytes_needed = VescFrame::VESC_MIN_FRAME_SIZE; + if (!buffer_.empty()) { + // search buffer for valid packet(s) + auto iter = buffer_.begin(); + auto iter_begin = buffer_.begin(); + while (iter != buffer_.end()) { + // check if valid start-of-frame character + if (VescFrame::VESC_SOF_VAL_SMALL_FRAME == *iter || + VescFrame::VESC_SOF_VAL_LARGE_FRAME == *iter) + { + // good start, now attempt to create packet + std::string error; + VescPacketConstPtr packet = + VescPacketFactory::createPacket(iter, buffer_.end(), &bytes_needed, &error); + if (packet) { + // good packet, check if we skipped any data + if (std::distance(iter_begin, iter) > 0) { + std::ostringstream ss; + ss << "Out-of-sync with VESC, unknown data leading valid frame. Discarding " << + std::distance(iter_begin, iter) << " bytes."; + error_handler_(ss.str()); } + // call packet handler + packet_handler_(packet); + // update state + iter = iter + packet->frame().size(); + iter_begin = iter; + // continue to look for another frame in buffer + continue; + } else if (bytes_needed > 0) { + // need more data, break out of while loop + break; // for (iter_sof... + } else { + // else, this was not a packet, move on to next byte + error_handler_(error); } - - iter++; } - // if iter is at the end of the buffer, more bytes are needed - if (iter == buffer_.end()) { - bytes_needed = VescFrame::VESC_MIN_FRAME_SIZE; - } + iter++; + } - // erase "used" buffer - if (std::distance(iter_begin, iter) > 0) { - std::ostringstream ss; - ss << "Out-of-sync with VESC, discarding " << std::distance(iter_begin, iter) << " bytes."; - error_handler_(ss.str()); - } - buffer_.erase(buffer_.begin(), iter); + // if iter is at the end of the buffer, more bytes are needed + if (iter == buffer_.end()) { + bytes_needed = VescFrame::VESC_MIN_FRAME_SIZE; } - // Only attempt to read every 5 ms - std::this_thread::sleep_for(std::chrono::milliseconds(5)); + + // erase "used" buffer + if (std::distance(iter_begin, iter) > 0) { + std::ostringstream ss; + ss << "Out-of-sync with VESC, discarding " << std::distance(iter_begin, iter) << " bytes."; + error_handler_(ss.str()); + } + buffer_.erase(buffer_.begin(), iter); } } @@ -155,9 +152,14 @@ void VescInterface::Impl::connect(const std::string & port) serial_driver_->init_port(port, *device_config_); if (!serial_driver_->port()->is_open()) { serial_driver_->port()->open(); + serial_driver_->port()->async_receive( + std::bind( + &VescInterface::Impl::receive_callback, this, std::placeholders::_1, + std::placeholders::_2)); } } + VescInterface::VescInterface( const std::string & port, const PacketHandlerFunction & packet_handler, @@ -205,12 +207,6 @@ void VescInterface::connect(const std::string & port) ss << "Failed to open the serial port " << port << " to the VESC. " << e.what(); throw SerialException(ss.str().c_str()); } - - // start up a monitoring thread - impl_->packet_thread_run_ = true; - impl_->packet_thread_ = std::unique_ptr( - new std::thread( - &VescInterface::Impl::packet_creation_thread, impl_.get())); } void VescInterface::disconnect() @@ -219,9 +215,7 @@ void VescInterface::disconnect() if (isConnected()) { // bring down read thread - impl_->packet_thread_run_ = false; requestFWVersion(); - impl_->packet_thread_->join(); impl_->serial_driver_->port()->close(); } }